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Abstract 

The  military  depends  on  the  Global  Positioning  System  (GPS)  for  a  wide  array 
of  advanced  weaponry  guidance  and  precision  navigation  systems.  Increased  numbers 
of  military  operations  are  conducted  in  GPS  degraded  or  denied  environments  (e.g., 
urban  canyons  or  inside  buildings).  Lack  of  GPS  access  makes  precision  navigation  in 
these  environments  very  difficult.  Inclusion  of  inertial  sensors  in  existing  navigation 
systems  provides  short-term  precision  navigation  during  periods  where  GPS  cannot  be 
used,  but  drifts  signihcantly  over  long-term  navigation.  Development  of  the  navigation 
system  presented  in  this  thesis  is  motivated  by  the  need  for  inertial  sensor  drift- 
constraint  for  precision  navigation  in  degraded  and  denied  GPS  environments.  The 
navigation  system  developed  in  this  thesis  consists  of  inertial  sensors,  a  simulated 
barometer,  three  Raytheon  DH500  radios,  and  a  stereo-camera  image-aiding  system. 
The  Raytheon  DH500  is  a  combat  communication  radio  which  also  provides  range 
measurements  between  radios  within  the  local  radio  network.  The  measurements 
from  each  sensor  are  fused  together  with  an  extended  Kalman  filter  to  estimate  the 
navigation  trajectory.  Residual  monitoring  and  the  Sage-Husa  adaptive  algorithm  are 
individually  tested  in  the  Kalman  hlter  range  update  algorithm  to  help  improve  the 
radio  range  positioning  performance.  The  navigation  system  in  this  thesis  is  shown 
to  provide  long-term  inertial  sensor  drift-constraint  with  position  errors  as  low  as  3 
meters. 


IV 


Acknowledgements 

First,  I  acknowledge  Jesus  my  Savior  for  prompting  me  to  come  here  to  get  my  masters 
and  then  giving  me  the  strength  and  motivation  to  complete  this  work.  You  are  my 
Rock. 

Next,  Fd  like  to  thank  my  advisor  for  challenging  me  and  giving  me  the  oppor¬ 
tunity  to  do  my  best. 

I  would  also  like  to  thank  the  good  friends  I  made  at  AFIT  for  slugging  through 
this  thesis  program  alongside  me.  Their  humor  kept  me  sane  and  proof-reading  skills 
really  helped  to  rehne  what  you  read  today.  I  also  really  appreciate  the  work  of 
the  ANT  Center  staff  for  all  the  software  and  hardware  building  and  long  hours  of 
debugging  that  was  necessary  to  get  good  data  for  my  thesis. 

Last,  but  by  no  means  least,  I  would  like  to  thank  my  whole  family  back  home 
for  their  unwaivering  support  through  this  entire  time.  I’m  hnally  done  with  school! 
(for  now) 


Erich  Lichtfuss 


V 


Table  of  Contents 

Page 

Abstract .  iv 

Acknowledgements .  v 

List  of  Figures  .  viii 

List  of  Tables .  xv 

List  of  Abbreviations .  xvi 

I.  Introduction  .  1 

1.1  Current  Technology  .  1 

1.2  Proposed  Solution  .  2 

1.3  Organization .  3 

II.  Background .  4 

2.1  Notation .  4 

2.2  Coordinate  Systems  and  Transformations  .  5 

2.2.1  World  Geodic  System  1984  .  5 

2.2.2  Coordinate  Systems .  5 

2.2.3  Coordinate  System  Transformations .  8 

2.3  State  Estimation  and  Sensor  Fusion .  11 

2.3.1  The  Linear  Kalman  Filter .  11 

2.3.2  Extended  Kalman  Filter .  15 

2.4  Inertial  Navigation .  21 

2.4.1  INS  Sensors .  21 

2.4.2  INS  Mechanization .  23 

2.4.3  INS  Error  Sources .  24 

2.4.4  Computation  Errors  .  26 

2.5  Radio  Positioning  Methods  .  26 

2.5.1  Range- Aided  Navigation  Survey .  27 

2.5.2  Time  of  Arrival .  29 

2.5.3  Angle  of  Arrival .  30 

2.5.4  Received  Signal  Strength .  31 

2.6  Multipath  Mitigation .  32 

2.7  Baro-Altitude  Aiding .  36 

2.8  Chi  Square  Statistics .  38 

2.9  Adaptive  Filter  Algorithms  .  39 

vi 


Page 

2.9.1  Residual  Monitoring .  40 

2.9.2  Sage-Husa  Algorithm .  42 

III.  Methodology .  48 

3.1  Navigation  System .  48 

3.1.1  Sensor  Platform .  48 

3.1.2  EKF  Implementation .  51 

3.2  Data  Collections .  56 

3.2.1  Stationary  Outdoor  Collect  .  56 

3.2.2  Moving  Outdoor  Collect .  58 

3.2.3  Stationary  Indoor  Collects .  59 

3.2.4  Moving  Indoor  Collects .  62 

IV.  Results  .  66 

4.1  Radio  Performance  Characterization .  66 

4.1.1  Outdoor  Characterization .  66 

4.1.2  Indoor  Characterization  .  70 

4.2  Outdoor  Moving  Analysis .  77 

4.2.1  INS  Data  Only,  No  Filter  Bias  Estimation  ...  78 

4.2.2  Enable  Filter  Bias  Estimation .  82 

4.2.3  Add  Vertical  Channel  Constraint  and  Range  Mea¬ 
surements  .  85 

4.2.4  Residual  Monitoring  and  Sage-Husa .  92 

4.3  Indoor  Moving  Analysis .  102 

4.3.1  Residual  Monitoring  and  Sage-Husa .  103 

4.3.2  Stereo  Image  Aiding .  112 

V.  Conclusions  and  Future  Work .  117 

5.1  Conclusions .  117 

5.2  Future  Work  .  118 

5.3  Closing  .  120 


Bibliography 


121 


Figure 


List  of  Figures 


Page 


2.1.  WGS84  Coordinate  System  Definition.  The  three-axis,  orthogo¬ 
nal  coordinate  system  aligns  the  ;2-axis  with  the  north  pole  and 


the  x-axis  with  the  Greenwich  meridian  [1] .  6 

2.2.  Common  Frames  of  Reference.  Depicts  Inertial,  Earth,  Naviga¬ 
tion,  and  Body  reference  frames  [25] .  8 


2.3.  Block  Diagram  for  Nonlinear  Integration.  The  fnll  state  estimate 
X  and  control  input  u  are  used  to  calculate  the  change  in  the  full 
state  estimate  denoted  as  x.  The  integration  step  accumulates 

the  changes  to  produce  a  whole  state  estimate .  19 

2.4.  Positioning  Based  on  Time  of  Arrival  for  a  3-Dimensional  Solu¬ 
tion.  The  time  of  arrival  for  the  radio  signals  sent  between  each 
mobile  and  base  station  pair  presents  a  measurement  of  the  slant 

range  distance  between  the  radio  antennas  [14] .  30 

2.5.  Positioning  Based  on  Angle  of  Arrival  for  a  2-Dimensional  Solu¬ 
tion.  Strict  dehnitions  of  radio  signal  arrival  angles  are  reqnired 

to  perform  angle  of  arrival  radio  positioning  [14] .  30 

2.6.  Positioning  Based  on  Received  Signal  Strength  for  3-Dimensional 
Solution.  Un-modeled  RF  channel  signal  reduction  between  each 
mobile  and  base  radio  pair  introduces  signihcant  uncertainty  into 

the  position  solntion  [14] .  32 

2.7.  Depiction  of  RF  Mnltipath.  The  signal  from  the  transmitter 
decreases  in  strength  with  each  reflection.  The  reflected  signals 
travel  a  longer  total  distance  to  the  receiver  which  adds  a  positive 

error  bias  to  the  slant  range  measurement .  33 

2.8.  Illustration  of  the  Three  Scatterer  Models  [3].  The  scatterers 
lie  on  the  ring  for  the  ROS.  For  the  DOS  model,  the  scatterers 
are  nniformly  distributed  within  the  ring.  The  clipped  Gaussian 
model  positions  the  scatterers  in  a  Gaussian  distribution  centered 
aronnd  the  master  station.  .  . 

viii 


35 


Page 


Figure 

2.9.  Barometer  Error  Model.  The  FOGM  describes  barometer  error 
and  the  white  Gaussian  noise  source  v{t)  describes  measurement 
process  noise .  38 

3.1.  Overview  of  Navigation  System  Setup.  Note  the  two  stationary 
DH500  radios  BSl  and  BS2  provide  slant  range  measurements  ri 

and  r2  for  constraint  of  the  navigation  drift .  49 

3.2.  Position  &  Orientation  of  Mobile  Sensors  on  Vehicle.  A  location 

vector  lb  and  orientation  DGM  Cl  is  dehned  for  each  sensor 
relative  to  the  &-frame.  The  INS  sensor  s-frame  origin  is  dehned 
to  be  the  origin  of  the  6-frame  and  INS  s-frame  orientation  is 
dehned  to  align  with  the  6-frame .  50 

3.3.  Outdoor  Stationary  Data  Gollection  Setup.  The  displayed  loca¬ 
tions  of  the  base  stations  and  mobile  station  are  chosen  to  provide 

the  clearest  LOS  and  low  RF  interference  between  the  radios.  .  57 

3.4.  Outdoor  Moving  Data  Gollection  Setup.  The  locations  of  the 
base  stations  do  not  change  from  the  stationary  data  collection 
to  the  moving  collection.  The  sensor  platform  is  moved  along 

the  track  indicated  by  the  arrows .  58 

3.5.  Outdoor  Radio  Positions  for  Indoor  Data  Gollections.  The  po¬ 
sition  of  the  outside  radios  simulate  potential  locations  of  radio 
base  stations  used  in  the  held.  See  Figure  3.6  for  indoor  hallway 

layout .  60 

3.6.  Indoor  Hallway  Surveyed  Locations  for  Stationary  Data  Gollec¬ 
tions.  Locations  are  displayed  for  north  and  south  stationary 
data  collections.  Note  the  layers  of  building  walls  illustrated  by 
the  building  layout.  These  walls  attenuate  the  radio  RF  signals 

and  rehect  the  RF  signals  causing  RF  interference .  61 

3.7.  North-South  Indoor  Moving  Trajectory  Illustration.  The  sur¬ 
veyed  points  provide  position  “truth”  data  to  compare  against 
the  EKF  post-process  indoor  trajectory.  Note  changes  in  trajec¬ 
tory  direction  between  Figure  3.7  (a)  and  3.7  (b)  indicated  by 

the  black  arrow .  62 


IX 


Figure 

Page 

3.8. 

Square  Indoor  Moving  Trajectory  Illustration.  The  square  indoor 
moving  trajectory  provides  variation  in  RF  interference  and  sig¬ 
nal  attenuation . 

64 

4.1. 

Outdoor  Range  Error  Plot.  Spread  of  range  errors  is  due  to  noise 
in  the  range  measurement  system . 

67 

4.2. 

Outdoor  Range  Error  Between  Radio  Pair  1.  Note  how  outliers 
at  30  and  65  meters  create  a  long-tailed  distribution . 

68 

4.3. 

Outdoor  Range  Error  Between  Radio  Pair  2.  Note  how  range 
error  distribution  appears  to  reasonably  fit  Gaussian  overlay.  . 

69 

4.4. 

Indoor  North  Range  Error  Plot.  A  large  number  of  extreme 

outliers  exist  above  200  meters  due  to  RF  channel  interference 

from  the  building . 

71 

4.5. 

Indoor  South  Range  Error  Plot.  The  ranges  are  reasonable  con¬ 
tained  below  100  meters  as  compared  with  the  extreme  outliers 
shown  in  Pigure  4.4 . 

72 

4.6. 

Indoor  North  Range  Error  Between  Radio  Pair  1.  Note  the  long 
tail  on  the  range  error  distribution  due  to  RF  interference.  .  .  . 

73 

4.7. 

Indoor  North  Range  Error  Between  Radio  Pair  2.  Note  the  ex¬ 
treme  range  errors  due  to  RF  signal  interference . 

73 

4.8. 

Indoor  South  Range  Error  Between  Radio  Pair  1.  Note  the  pos¬ 
itive  tail  on  distribution  due  to  RF  interference . 

75 

4.9. 

Indoor  South  Range  Error  Between  Radio  Pair  2.  Note  the  close 
£t  of  the  histogram  to  the  normal  distribution  overlay . 

75 

4.10. 

Position  Error  for  Data  Collect  “moving.lb.”  Only  INS  data  is 
used  in  the  filter  and  the  bias  estimation  capability  is  disabled. 
Observe  the  very  high  hlter  position  uncertainty  which  exceeds 
10,0000  meters.  This  is  typical  for  a  tactical-grade  INS,  but  does 
not  provide  usable  long-term  navigation  capability . 

79 

4.11. 

Attitude  Error  for  Data  Collect  “moving_lb.”  Only  INS  data  is 
used  in  the  hlter  and  the  bias  estimation  capability  is  disabled. 
Note  the  large  attitude  uncertainty  of  87  mili-radians  (±5.0°).  . 

80 

X 


Figure 


Page 


4.12.  INS  Data  only  Horizontal  Trajectory  Comparison  for  the  four 
Outdoor  Moving  Data  Collections.  Note  the  variety  in  the  amount 

of  drift  for  each  data  set .  81 

4.13.  Position  Error  for  Data  Collection  “moving_lb.”  The  EKF  uses 

only  INS  data,  but  with  bias  estimation  enabled.  Note  the  factor 

of  100  reduction  in  hlter  uncertainty  for  the  position  stated  as 
compared  with  Figure  4.10  where  bias  estimation  is  disabled.  .  82 

4.14.  Attitude  Error  for  Data  Collect  “moving_lb.”  The  EKF  uses 

only  INS  data,  but  with  bias  estimation  enabled.  Note  the  factor 

of  100  decrease  in  hlter  uncertainty  for  the  attitude  states  when 
compared  with  no  bias  estimation  as  presented  in  Figure  4.11.  .  83 

4.15.  Accelerometer  Bias  Estimation  for  Data  Collection  “moving_lb.” 

The  hlter  is  given  only  INS  data  with  bias  estimation  enabled. 

The  low  uncertainty  of  the  bias  estimate  is  due  to  the  length  of 

the  hlter  alignment .  84 

4.16.  Gyroscope  Bias  Estimation  for  Data  Collection  “moving.lb.”  The 
hlter  is  given  only  INS  data  with  bias  estimation  is  enabled.  The 
low  uncertainty  of  the  bias  estimate  is  due  to  the  length  of  the 

hlter  alignment .  85 

4.17.  INS  Data  with  Bias  Estimation  Horizontal  Trajectory  Compar¬ 
ison.  Signihcant  reduction  in  trajectory  drift  is  observed  with 
EKF  bias  estimation  enabled.  The  signihcant  drift  in  data  col¬ 
lect  “moving_2b”  results  from  large  attitude  anomalies  which  add 

to  the  accumulation  of  INS  attitude  error .  86 

4.18.  Range  Residual  Analysis  for  Data  Collect  “moving_lb.”  This 
graphs  shows  the  initial  choice  of  8  meters  for  the  EKF  radio 
range  model  uncertainty  is  too  low.  The  actual  uncertainty  of 

the  range  measurements  is  higher .  87 

4.19.  Range  Residual  Analysis  for  Data  Collect  “moving_lb.”  Re¬ 
tuned  EKF  radio  range  model  with  uncertainty  dehned  as  55 
meters.  The  updated  model  uncertainty  now  hts  the  actual  range 
measurement  data  as  is  shown  by  70-80%  of  the  range  residuals 

falling  within  the  1-a  residual  covariance  bounds .  89 


XI 


Page 


Figure 

4.20.  Position  Error  for  Data  Collect  “moving_lb.”  The  EKF  utilizes 

bias  estimation  with  INS,  radio  range,  and  vertical  channel  con¬ 
straint  data.  The  radio  model  uncertainty  has  been  re-tuned  to 
55  meters.  Note  the  constrained  uncertainty  for  the  hlter  position 
states.  Also,  note  the  hlter  position  errors  do  not  signihcantly 
exceed  the  l-a  hlter  uncertainty  bounds,  indicating  a  correctly 
tuned  hlter .  90 

4.21.  Horizontal  Trajectory  Comparison.  Range  measurements  and 

vertical  channel  constraint  data  are  added  to  the  INS  data  with 
EKF  bias  estimation  enabled.  Overall,  the  additional  data  pro¬ 
vides  notable  trajectory  drift  constraint .  91 

4.22.  Range  Residual  Analysis  for  Data  Collect  “moving_2b.”  This 

graph  shows  the  range  residuals  before  residual  monitoring  is 
applied.  Note  the  large  negative  range  residuals  which  occur 
towards  the  beginning  of  the  trajectory  for  radio  pair  2 .  94 

4.23.  Range  Residual  Analysis  for  Data  Collection  “moving_2b.”  The 

graph  shows  the  range  residuals  that  are  rejected  by  the  residual 
monitoring  algorithm.  These  residuals  exceeded  the  3-a  thresh¬ 
old  and  are  not  use  to  update  the  EKF’s  position  states .  95 

4.24.  Average  RMS  Trajectory  Error  for  a  Range  of  Sage-Husa  Mem¬ 

ory  Factors.  Note  how  each  data  set  responds  differently  to  the 
same  range  of  memory  factors.  Average  RMS  trajectory  error 
for  the  standard  EKF  range  update  algorithm  is  shown  for  com¬ 
parison .  97 

4.25.  Sage-Husa  Estimate  Analysis  for  Data  Collection  “moving_2b.” 

Note  how  the  measurement  noise  variance,  or  model  uncertainty, 
adjusts  to  account  for  changes  in  the  residual  distribution.  The 
measurement  noise  mean,  or  range  bias,  slowly  tracks  with  the 
average  range  residual  value  providing  an  estimate  of  the  bias  in 

the  range  measurements .  98 

4.26.  Residual  Monitoring  and  Sage-Husa  Horizontal  Trajectory  Com¬ 
parison.  Residual  monitoring  only  impacts  data  set  “moving_2b” 
due  to  the  large  range  residuals.  The  Sage-Husa  adaptive  algo¬ 
rithm  has  a  variety  of  results  dependent  on  the  amount  of  INS 

drift  and  quality  of  range  measurements .  100 

xii 


Figure 


Page 


4.27.  Residual  Monitoring  and  Sage-Husa  Horizontal  Trajectory  Com¬ 

parison  for  North-South  Indoor  Data  Sets.  Residual  monitoring 
does  not  provide  additional  drift  constraint  due  to  the  high  range 
model  uncertainty.  Sage-Husa  provides  a  reduction  in  drift  by  re¬ 
ducing  the  range  model  uncertainty  for  range  measurements  with 
reduced  RF  interference  as  shown  in  Figures  4.27  (a)  and  4.27  (c).  106 

4.28.  Filter  Position  States  Uncertainty  Comparison  for  Indoor  Moving 
Data  Set  “North-South  la.”  Notice  that  the  filter  uncertainty  for 

the  north  and  east  axes  is  similar  for  each  set  of  algorithms.  .  .  107 

4.29.  Residual  Monitoring  and  Sage-Husa  Horizontal  Trajectory  Com¬ 
parison  for  Square  Data  Sets.  Residual  monitoring  provides  some 
drift  constraint  in  Figure  4.29  (d),  but  does  not  affect  the  remain¬ 
ing  data  sets  due  to  the  high  range  model  uncertainty.  Sage- 
Husa  provides  signihcant  drift  constraint,  allowing  range  mea¬ 
surements  with  low  RF  interference  to  more  strongly  correct  the 

hlter  position  estimate .  108 

4.30.  Sage-Husa  Estimate  Analysis  for  Data  Collection  “Square  3.” 

The  range  model  uncertainty  estimate,  measurement  noise  vari¬ 
ance,  reduces  the  effect  of  very  poor  range  measurements  through 
the  increase  of  the  model  uncertainty.  Less  RF  interference  is 
present  towards  the  end  of  the  trajectory  and  the  signihcant  re¬ 
duction  in  model  uncertainty  allows  these  range  measurements 

to  strongly  constrain  the  trajectory  drift .  110 

4.31.  Filter  Position  States  Uncertainty  Comparison  for  Indoor  Moving 
Data  Set  “Square  1.”  Notice  that  the  hlter  uncertainty  for  the 
north  and  east  axes  varies  based  on  the  algorithms  employed. 

The  north  axis  uncertainty  is  larger  than  the  east  axis  for  residual 
monitoring  and  the  standard  algorithm .  Ill 

4.32.  Image-Aiding  Horizontal  Trajectory  Comparison  for  North-South 

data  sets.  The  addition  of  image-aiding  provides  additional  va¬ 
riety  in  both  the  reduction  of  and  addition  to  existing  trajectory 
drift.  The  sparse,  hat  hallways  provide  a  minimal  number  of 
image  features  for  the  image-aiding  system  to  track  resulting  in 
overall  poor  performance .  113 

xiii 


4.33.  Image-Aiding  Horizontal  Trajectory  Comparison  for  the  SQR 
Data  Set.  The  results  from  image-aiding  are  varied.  Round¬ 
ing  hallway  corners  during  the  trajectory  present  a  significant 
source  of  image-aiding  error.  Camera  view  obstructions  such  as 
people  and  objects  in  the  hallways  cause  further  image-aiding 
errors  which  results  in  a  reduction  of  trajectory  drift . 


List  of  Tables 


Table 

Page 

4.1. 

Statistics  for  Radio  Pair  1  Range  Error.  The  range  distribution 

has  a  non-zero  mean  and  does  not  conform  to  the  Gaussian  dis¬ 
tribution . 

68 

4.2. 

Statistics  for  Radio  Pair  2  Range  Error.  The  range  distribution 
is  approximately  zero-mean  and  conforms  to  the  Gaussian  dis¬ 
tribution . 

70 

4.3. 

Statistics  for  North,  Radio  Pair  1  Range  Error.  The  range  dis¬ 
tribution  has  a  positive  mean,  due  to  RE  interference,  and  does 

not  conform  to  the  Gaussian  distribution . 

74 

4.4. 

Statistics  for  North,  Radio  Pair  2  Range  Error.  The  range  dis¬ 
tribution  has  a  large  positive  mean,  due  to  RE  interference,  and 

does  not  conform  to  the  Gaussian  distribution . 

74 

4.5. 

Statistics  for  South,  Radio  Pair  1  Range  Error.  The  range  dis¬ 
tribution  has  a  positive  mean,  due  to  RE  interference,  and  does 

not  conform  to  the  Gaussian  distribution . 

76 

4.6. 

Statistics  for  South,  Radio  Pair  2  Range  Error.  The  range  dis¬ 
tribution  has  a  positive  mean,  due  to  RE  interference,  and  does 

not  conform  to  the  Gaussian  distribution . 

76 

4.7. 

Total  Stationary  and  Moving  time  of  the  Outdoor  Moving  Data 

Sets . 

77 

4.8. 

Total  Stationary  and  Moving  time  of  the  Indoor  Moving  Data 

Sets . 

102 

XV 


List  of  Abbreviations 

Abbreviation  Page 

GPS  Global  Positioning  System .  1 

WGS84  World  Geodic  System  1984  .  5 

NGA  National  Geospatial-Intelligence  Agency .  5 

lERS  International  Earth  Rotation  Service .  5 

IRM  lERS  Reference  Meridian .  5 

IRP  lERS  Reference  Pole .  5 

NED  North  East  Down .  7 

NED  North  East  Down .  7 

DGM  Direction  Gosine  Matrix .  9 

EKE  Extended  Kalman  Filter .  15 

INS  Inertial  Navigation  System .  21 

MS  Mobile  Station .  27 

BS  Base  Station  .  27 

RE  Radio  Frequency .  27 

TOA  Time  of  Arrival .  27 

AO  A  Angle  Of  Arrival .  27 

RSS  Received  Signal  Strength .  27 

LOS  Line  Of  Sight .  29 

NLOS  Non-Line  Of  Sight .  29 

ROS  Ring  of  Scatterers  .  33 

DOS  Disk  of  Scatterers .  33 

IMM  Interacting  Multiple  Model  .  33 

GRLB  Gramer-Rao  Lower  Bound .  34 

G-GRLB  Generalized  Gramer-Rao  Lower  Bound .  34 

FOGM  First-Order  Gauss  Markov .  37 


XVI 


Abbreviation  Page 

GOF  Goodness  Of  Fit .  38 

DGPS  Differential  GPS .  48 

VAN  Vision-Aided  Navigation .  51 

ANT  Advanced  Navigation  Technology .  63 


xvii 


Non-GPS  Navigation  Using  Vision- Aiding 


AND 

Agtive  Radio  Range  Measurements 

I.  Introduction 

This  thesis  presents  a  research  effort  focused  on  the  fusion  of  radio  range  mea¬ 
surements  with  inertial  and  image  data  to  provide  precision  navigation  without 
the  use  of  the  global  positioning  system  (GPS).  This  research  effort  is  motivated  by 
the  requirement  for  precision  navigation  in  environments  where  GPS  is  degraded  or 
denied.  Urban  and  indoor  environments  are  examples  of  places  where  GPS  signals 
are  degraded  or  denied. 

The  military  depends  on  GPS  for  a  wide  array  of  advanced  weaponry  guidance 
and  navigation  systems.  General  Schwartz  characterized  this  GPS  dependence  in  a 
2010  keynote  speech  as  an  “exploitable  vulnerability.”  [23]  Increasing  numbers  of  mili¬ 
tary  operations  are  being  conducted  in  so-called  urban  canyons  where  the  surrounding 
building  structures  block  direct  view  of  the  GPS  satellites.  The  tall  structures  gener¬ 
ate  RF  interference,  obscuring  line-of-sight  view  to  the  GPS  satellites.  This  severely 
degrades  the  GPS  navigation  capability. 

General  Schwartz  stated  that  our  military  force  must  “reduce  its  dependence 
on  GPS-aided  precision  navigation  and  timing.”  He  then  directed  research  efforts  to 
explore  new  technologies  which  will  provide  “less  vulnerable,  yet  equally  precise”  nav¬ 
igation  capabilities.  These  navigation  capabilities  must  enable  our  forces  “to  operate 
in  GPS-denied  environments”  during  future  operations. 

1 . 1  Current  Technology 

GPS-aided  inertial  navigation,  indoor  radio  navigation  systems,  and  image- 
aided  navigation  systems  are  three  examples  of  current  navigation  technologies.  For 
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the  GPS-aided  inertial  navigation  system,  the  GPS  provides  long-term  position  sta¬ 
bility  and  the  inertial  sensors  provide  short-term  position  precision  [20].  When  GPS 
access  is  denied,  the  inertial  position  solution  drifts  signihcantly  providing  an  unusable 
navigation  solution. 

The  lack  of  indoor  access  to  GPS  is  addressed  in  the  commercial  community  by 
development  of  radio  navigation  systems  [10].  These  systems  consist  of  radio  nodes 
placed  at  known  locations  throughout  the  building.  Indoor  navigation  is  then  based 
on  proximity  to  each  of  the  radio  nodes.  The  indoor  radio-based  positioning  system 
provides  GPS-like  position  capability,  but  requires  prior  knowledge  of  the  building  and 
prior  placement  of  the  radio  nodes.  Unfortunately,  this  is  not  practical  in  a  combat 
environment. 

Navigation  without  GPS  in  an  unknown  environment  is  addressed  by  stereo¬ 
camera  image-aiding  which  is  used  to  constrain  inertial  sensor  drift.  Such  an  image- 
aided  navigation  system  is  developed  by  Veth  [26]  and  is  shown  to  provide  very  good 
indoor  navigation  without  prior  environment  knowledge.  However,  this  method  drifts 
unconstrained  when  the  camera  system  is  unable  to  hnd  enough  good  image  features 
to  track. 

1.2  Proposed  Solution 

The  proposed  radio-range  aided  navigation  system  addresses  the  issues  of  in¬ 
ertial  navigation  position  drift  with  minimal  radio  infrastructure.  The  navigation 
system  is  composed  of  an  inertial  navigation  system,  a  simulated  barometer  to  con¬ 
straint  vertical  navigation  drift,  the  Raytheon  DH500  radio  range  system,  and  the 
stereo-camera  image-aiding  system  developed  by  Veth  [26].  Three  DH500  radios  are 
provided  for  this  thesis.  Two  radios  are  positioned  at  known  positions  which  simulate 
potential  radio  base  stations  on  troop  transport  vehicles.  The  third  radio  is  located 
with  the  inertial  sensors  and  camera  system  which  would  be  carried  by  the  individual 
soldier  or  on  a  mobile  platform. 
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To  test  the  navigation  system,  the  sensor  measurements  are  combined  using  an 
extended  Kalman  hlter,  presented  in  Section  2.3.2.  Using  measurement  updates  from 
each  sensor,  the  Kalman  hlter  estimates  the  position,  velocity,  and  attitude  of  the 
navigation  system.  Additional  algorithms  including  residual  monitoring,  presented  in 
Section  2.9.1,  and  the  Sage-Husa  adaptive  algorithm,  presented  in  Section  2.9.2,  are 
used  to  rehne  the  Kalman  hlter’s  usage  of  the  radio  range  measurements.  Coupling 
these  sensors  together  provides  a  better  navigation  solution. 

1 . 3  Organization 

The  remainder  of  this  thesis  is  organized  as  follows.  Chapter  II  covers  back¬ 
ground  topics  integral  to  understanding  the  navigation  system  implemented  in  this 
thesis.  Topics  include  coordinate  frames,  sensor  fusion,  radio  positioning  methods, 
and  adaptive  hlter  techniques.  The  methods  used  to  implement  and  test  the  nav¬ 
igation  system  are  presented  in  Chapter  III  with  results  detailed  and  analyzed  in 
Chapter  IV.  Chapter  V  concludes  this  thesis  with  hnal  remarks  and  suggestions  for 
future  areas  of  exploration. 
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II.  Background 


The  background  chapter  presents  the  mathematical  and  technical  foundation  nec¬ 
essary  for  proper  implementation  of  the  radio  aided  navigation  system.  The 
chapter  begins  with  the  dehnition  of  the  mathematical  notation  used  in  this  docu¬ 
ment.  Next,  common  coordinate  systems  and  transformation  methods  are  covered. 
Presentation  follows  of  the  sensors,  mechanization,  and  errors  of  the  inertial  navi¬ 
gation  system.  Next,  evaluation  of  related  work  in  radio  positioning  methods  with 
common  techniques  to  compute  position  are  presented.  Errors  dne  to  RF  multi- 
path  are  presented  along  with  algorithms  to  estimate  and  mitigate  multipath  effects. 
Next,  baro- altitude  aiding  addresses  the  lack  of  vertical  navigation  channel  observ¬ 
ability  present  with  radio  navigation.  The  Chi  Sqnared  statistic  aids  in  evaluation 
of  range  data  errors  against  the  Gaussian  probability  density  fnnction.  The  chapter 
concludes  with  adaptive  hltering  algorithms  to  improve  the  radio  range  modeling. 

2. 1  Notation 

The  mathematical  notation  used  throughout  this  thesis  is  presented  in  this 
section.  The  notation  provided  in  [26]  provides  guidance  for  the  following  dehnitions. 

•  Scalar  qnantities  are  represented  with  lower  or  upper  case  italic  symbols.  Ex¬ 
amples  include  scale  factors  such  as  A  and  d. 

•  Vector  quantities  are  represented  with  lower  case  boldface  italic  symbols.  Ex¬ 
amples  include  the  state  vector  x  and  measurement  vector  z. 

•  Matrix  quantities  are  represented  with  npper  case  boldface  italic  symbols.  Ex¬ 
amples  inclnde  the  state  transition  matrix  $  and  measurement  matrix  H. 

•  Estimated  variables  are  represented  with  a  hat  accent.  Examples  include  the 
estimate  of  the  state  vector  x  and  estimate  of  the  measurement  vector  z. 

•  Variables  with  error  are  represented  with  a  tilde  accent.  Examples  inclnde 
the  slant  range  measurement  from  ranging  radios  r. 
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•  Direction  cosine  matrices  provide  rotation  between  coordinate  frames  de¬ 
noted  by  The  subscript  indicates  the  source  frame  and  the  superscript 
indicates  the  destination  frame. 

2.2  Coordinate  Systems  and  Transformations 

Navigation  is  performed  relative  to  a  frame  of  reference.  The  frame  of  reference 
is  dependent  on  the  environment  where  the  navigation  is  performed.  The  Earth’s 
surface,  the  streets  of  a  city,  and  the  interior  of  a  building  present  examples  of  en¬ 
vironments  where  the  signihcance  of  a  navigation  solution  is  dehned  relative  to  the 
present  environment.  Knowledge  of  the  position  and  orientation  of  sensors  mounted 
on  a  vehicle  enable  the  sensor  data  to  correctly  aid  the  navigation  solution. 

The  coordinate  systems  and  transformations  presented  in  this  section  provide 
a  framework  to  dehne  the  position  and  orientation  of  the  navigation  state  and  as¬ 
sociated  sensors  relative  to  the  applicable  environment.  This  section  continues  with 
presentation  of  the  World  Geodic  System  1984  specihcation,  detailed  dehnitions  for 
common  reference  frames,  and  coordinate  system  transformations. 

2.2.1  World  Geodic  System  1984-  The  World  Geodic  System  1984  specihca¬ 
tion  (WGS84)  is  a  worldwide  reference  frame  which  enables  precise  quantihcation  of 
locations  on  the  Earth  [1].  The  WGS84  specihcation  represents  the  result  of  ongoing 
ehorts  by  the  National  Geospatial-Intelligence  Agency  (NGA)  to  further  rehne  and 
detail  the  surface  of  the  Earth.  WGS84  is  a  three-dimensional  right-handed  coor¬ 
dinate  system  as  shown  in  Figure  2.1.  The  x-axis  passes  through  the  International 
Earth  Rotation  Service  (lERS)  Reference  Meridian  (IRM)  which  is  analogous  to  the 
Greenwich  meridian.  The  ;2-axis  passes  through  the  lERS  Reference  Pole  (IRP)  with 
is  analogous  to  the  north  pole.  The  y-axis  is  orthogonal  to  both  the  x  and  2:  axes. 

2.2.2  Coordinate  Systems.  Goordinate  systems  provide  a  structure  to  char¬ 
acterize  the  location  and  orientation  of  a  vehicle  and  sensors  in  space.  This  enables 
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lERS  Reference  Pole  (IRP) 


Figure  2.1:  WGS84  Coordinate  System  Definition.  The  three-axis,  orthogonal  coor¬ 
dinate  system  aligns  the  ;2-axis  with  the  north  pole  and  the  x-axis  with  the  Greenwich 
meridian  [1]. 

changes  in  the  position  and  orientation  of  a  vehicle  to  be  qnantihed  and  nnderstood. 
A  bnlleted  list  follows  with  formal  coordinate  frame  dehnitions  [25,26]. 

•  Inertial  Frame  (/-frame):  The  /-frame  is  a  three-axis,  right-handed  coordinate 
system  with  no  predehned  origin  or  orientation.  The  inertial  frame  is  a  theoret¬ 
ical  reference  frame  which  does  not  accelerate  or  rotate.  Newton’s  laws  apply 
in  this  reference  frame. 

•  Earth-Centered  Frame  (i-frame):  The  Earth-centered  frame  is  a  three-axis  right- 
handed  coordinate  system  with  the  origin  located  at  the  center  of  the  Earth’s 
mass.  The  x  and  y  axes  lay  on  the  eqnator  and  do  not  rotate  relative  to  the 
fixed  stars.  The  i-frame  is  not  a  true  inertial  frame  since  it  accelerates  throngh 
space  with  the  Earth.  However,  for  navigation  applications  on  the  snrface  of 
the  Earth  it  can  be  considered  an  inertial  reference  frame.  Figure  2.2  presents 
the  Fframe  with  axes  labeled  Oxi-,  Oyi,  and  Ozi- 

•  Earth-Centered  Earth-Fixed  Frame  (e-frame):  The  Earth-centered  Earth-hxed 
frame  is  a  three-axis  right-handed  coordinate  system  with  the  frame  origin  lo¬ 
cated  at  the  center  of  the  Earth’s  mass.  The  e-frame  does  not  rotate  relative 
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to  the  Earth  with  the  x-axis  aligned  with  the  intersection  of  the  Greenwich 
meridian  and  the  equator.  The  z-axis  points  out  the  north  pole.  The  y-axis 
points  out  from  the  equator  90  degrees  east  longitude.  The  e-frame  is  a  Carte¬ 
sian  coordinate  system.  This  characteristic  allows  for  simplihcation  of  certain 
navigation  calculations.  Figure  2.2  presents  the  e-frame  origin  at  O  with  axes 
labeled  O^e,  Oye,  and  Oze- 

•  Vehicle-Fixed  Local-Level  Navigation  Frame  (n’-frame):  The  vehicle-hxed  local- 
level  navigation  frame  is  a  three-axis  right-handed  coordinate  system  with  the 
origin  dehned  relative  to  the  vehicle.  The  x,  y,  and  z  axes  are  dehned  to  point 
towards  north,  east,  and  down  (NED),  respectively.  The  down  direction  is 
dehned  to  align  with  the  the  local  gravity  vector.  The  vehicle-hxed  navigation 
frame  is  diherentiated  from  the  Earth-hxed  navigation  frame  by  the  rotation  of 
the  vehicle-hxed  frame  relative  to  the  Earth’s  surface  as  dehned  by  the  rotation 
rate  This  rotation  is  discussed  further  in  Section  2.4.2. 

•  Earth-Fixed  Local-Level  Navigation  Frame  (n-frame):  The  Earth-hxed  local- 
level  navigation  frame  is  a  three-axis  right-handed  coordinate  system  with  the 
origin  dehned  relative  to  the  Earth.  In  general  the  n-frame  origin  is  dehned 
on  the  surface  of  the  Earth.  The  x,  y,  and  z  axes  are  dehned  to  point  towards 
north,  east,  and  down  (NED),  respectively.  The  down  direction  is  dehned  to 
align  with  the  local  gravity  vector.  The  n-frame  provides  simplihed  navigation 
trajectories  on  the  surface  of  the  Earth  within  a  local  area.  The  n-frame  is  not 
suited  to  long-distance  navigation.  Figure  2.2  presents  the  n-frame  origin  at  P 
with  axes  labeled  N,  E,  and  D. 

•  Vehicle  Body  Frame  (6-frame):  The  vehicle  body  frame  is  a  three-axis  right- 
handed  coordinate  system  hxed  to  the  vehicle.  The  origin  is  dehned  at  a  prede¬ 
termined  location  on  the  vehicle.  The  origin  typically  coincides  with  the  center 
of  the  on-board  navigation  sensor  suite.  The  x  and  y  axes  project  out  the  front 
and  right  side  of  the  vehicle  as  viewed  from  the  top.  The  z-axis  projects  out 
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Figure  2.2:  Common  Frames  of  Reference.  Depicts  Inertial,  Earth,  Navigation,  and 
Body  reference  frames  [25]. 


the  bottom  of  the  vehicle.  Changes  in  the  orientation  of  the  vehicle  6-frame  are 
dehned  as  roll,  pitch,  and  yaw. 

•  Sensor  Frame  (s-frame):  The  sensor  frame  is  a  three-axis  right-handed  coordi¬ 
nate  system  dehned  for  each  sensor  of  the  navigation  system.  The  origin  of  an 
individual  s-frame  is  dehned  relative  to  each  sensor.  For  the  purposes  of  this 
thesis,  the  various  s-frame  locations  and  orientations  are  dehned  relative  to  the 
6-frame.  The  orientation  of  the  s-frame  x,  y,  and  z  axes  is  unique  to  each  sensor. 


2.2.3  Coordinate  System  Transformations.  As  mentioned  previously,  navi¬ 
gations  systems  are  comprised  of  multiple  sensors.  In  order  to  use  information  from 
the  sensors,  the  orientation  of  the  sensor  relative  to  the  vehicle  body  frame  must  be 
known.  This  orientation  consists  of  the  roll,  pitch,  and  yaw  rotations  required  to 
align  the  sensor’s  coordinate  frame  with  the  vehicle  body  frame.  For  example,  the 
camera  sensor  views  the  environment  from  the  perspective  dehned  by  its  orientation. 
Dehnition  of  this  orientation  relative  to  the  vehicle  body  coordinate  systems  allows 
the  navigation  system  to  correctly  interpret  the  data  from  the  camera  for  inclusion 


into  the  navigation  solution.  This  section  continues  with  presentation  of  properties 
of  direction  cosine  matrices  (DCMs)  and  then  Euler  angles. 

The  DCM  dehnes  a  three-dimensional  matrix  rotation  between  two  coordinate 
frames.  A  vector  Vi  pre-multiplied  by  a  DCM  is  transformed  from  its  initial  coordinate 
frame  to  the  hnal  coordinate  frame  as  shown  in  Equation  (2.1). 

Vf  =  C{vi  (2.1) 

Several  DCMs  may  be  pre-multiplied  together  to  obtain  a  single  DCM  to  performs  the 
series  of  rotations  in  a  single  operation.  Equation  (2.2)  presents  a  series  of  rotations 
to  transform  a  vector  in  the  e-frame  Ve  into  the  s-frame  Vg  which  requires  passing 
through  the  n  and  6-frames. 


t;,  =  C>e  =  Cicicy,  (2.2) 

DCMs  have  two  important  characteristics  which  are  presented  in  the  following 
equations.  The  determinant  of  a  DCM  is  always  one 

Det(C*)  =  |C'|  =  1  (2.3) 

To  reverse  a  DCM  transformation,  take  the  transpose  or  inverse  of  the  matrix 

Cl  =  {Clr^  =  {Clf  (2.4) 

Euler  angles  dehne  three  rotations  about  orthogonal  axes.  These  rotations  relate 
the  orientation  of  a  source  coordinate  frame  to  the  destination  coordinate  frame. 
These  angles  must  be  applied  in  a  specihc  order  to  ensure  correct  hnal  coordinate 
frame  rotation  is  achieved.  First,  yaw  rotation  is  performed  about  the  2:-axis,  then 
pitch  rotation  about  the  the  y-axis,  and  hnally  roll  rotation  about  the  x-axis.  This 
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strict  order  of  rotation  ensures  the  correct  final  orientation  of  the  source  coordinate 
frame.  The  yaw  rotation  about  the  ;2-axis  is  measured  by  ip,  pitch  rotation  about  the 
y-axis  is  measured  by  6,  and  then  roll  rotation  about  the  x-axis  is  measured  by  (p. 
The  next  paragraph  presents  the  conversion  from  Euler  angles  to  the  DCM  format. 

The  three  rotations  presented  above  for  Euler  angles  may  be  translated  into  a 
single  DCM.  Conversion  from  Euler  angles  to  a  single  DCM  starts  with  creation  of 
one  DCM  for  each  Euler  angle  rotation.  Each  DCM  rotates  the  coordinate  frame  by 
the  corresponding  angle.  The  source  orientation  of  the  coordinate  frame  is  dehned  as 
orientation  1.  The  subsequent  rotations  are  dehned  as  orientations  2,  3,  and  4  where 
orientation  4  is  the  hnal  coordinate  frame  orientation.  A  combination  of  subscripts 
and  superscripts  are  used  to  indicate  the  order  of  transformations.  Equations  (2.5), 
(2.6),  and  (2.7)  present  the  matrices  that  correspond  to  each  of  the  three  rotations. 

cos  Ip  sin  Ip  0 

-sin  Ip  cos  Ip  0  (2.5) 

0  0  1 

cos  9  0  —sin  9 

0  1  0  (2.6) 

sin  9  0  cos  9 

1  0  0 

C'g  =  0  COS  (p  sin  (p  (2.7) 

0  —sin  (p  cos  (p 

Pre-multiplication  of  the  three  DCMs  results  in  a  single  DCM  which  performs  the 
same  rotation  in  a  single  step  as  shown  in  Equation  (2.8). 

Ct  =  CtClCl  (2.8) 
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2.3  State  Estimation  and  Sensor  Fusion 


The  radio- aided  navigation  system  developed  in  this  thesis  utilizes  multiple  sen¬ 
sors  each  with  unique  error  models  and  data  rates.  Each  sensor  provides  information 
about  the  current  vehicle  navigation  states  of  position,  velocity,  and  attitude.  Sen¬ 
sors  used  in  this  thesis  include:  an  inertial  navigation  system,  a  barometer,  a  camera 
system,  and  a  radio  range  system.  The  inertial  navigation  system  measures  how  the 
vehicle  accelerates  and  changes  attitude.  The  barometer  measures  air  pressure  which 
correlates  with  the  current  vehicle  altitude.  The  camera  system  tracks  landmarks  in 
the  visual  environment  which  provide  information  on  the  change  in  position  and  at¬ 
titude  of  the  vehicle.  Finally,  the  radio  range  system  provides  information  about  the 
position  of  the  vehicle  relative  to  each  of  the  ground  radio  stations.  The  Kalman  £1- 
ter  provides  an  estimation  algorithm  to  estimate  the  true  vehicle  states  and  optimally 
fuse  the  information  provided  by  each  sensor. 

In  reality,  the  “true”  vehicle  navigation  states  cannot  be  known  exactly,  but  they 
may  be  estimated.  The  Kalman  filter  uses  statistical  models  of  the  vehicle’s  motion 
and  sensors  to  estimate  a  statistically-optimal  solution  to  the  vehicles  navigation 
states  of  position,  velocity,  and  attitude.  The  vehicle  model  describes  how  the  vehicle 
moves  and  responds  to  changes  in  motion.  Each  sensor  model  maps  the  particular 
sensor  observations  to  the  filter  states.  The  sensor  model  also  describes  how  the  sensor 
corrupts  or  adds  error  to  the  “true”  position,  velocity,  or  attitude  of  the  vehicle.  This 
section  first  presents  the  linear  Kalman  filter  and  then  the  extended  Kalman  filter. 

2.3.1  The  Linear  Kalman  Filter.  The  Kalman  filter  is  composed  of  two 
classes  of  models  and  two  separate  algorithm  steps  [16].  The  vehicle  dynamics  model 
and  sensor  models  comprise  the  two  model  classes.  The  two  algorithms  consist  of 
time  propagation  and  measurement  update  steps.  Time  propagation  occurs  at  regular 
intervals  and  the  measurement  update  step  occurs  whenever  a  sensor  provides  a  mea¬ 
surement.  Specific  time  index  notation  used  to  indicate  variables  processed  through 
the  two  algorithm  steps  is  covered  at  the  beginning  of  each  algorithm  section.  The 
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next  section  presents  the  Kalman  filter  models  followed  by  the  time  propagation  and 
measurement  update  steps  in  separate  sections. 


2. 3. 1.1  Linear  Models.  The  vehicle  and  sensor  models  provide  the 
Kalman  hlter  with  prior  knowledge  of  the  vehicle  dynamics  and  sensor  properties  [16]. 
The  vehicle  dynamics  model  is  shown  in  Equation  (2.9) 


ti)x{ti)  +  Bd{ti)u{ti)  +  Gd{ti)wd{ti)  (2.9) 

where  x(ti+i)  is  the  new  state  estimate,  $(fj+i,  ti)  is  the  state  transition  matrix,  x{ti) 
is  the  current  state  estimate,  Bd{ti)  is  the  control  input  matrix,  u{ti)  contains  the 
control  input  vector,  and  Gd{ti)  is  the  noise  matrix  which  applies  each  noise  source 
dehned  in  the  vector  Wd{ti)  to  each  of  the  hlter  states.  The  noise  source  vector 
describes  the  uncertainty  of  the  vehicle  dynamics  model. 

Each  Kalman  hlter  sensor  model  maps  the  sensor’s  observation  to  the  hlter  states 
and  describes  the  uncertainty  of  the  model.  Equation  (2.10)  presents  the  sensor  model 

z{ti)  =  H{ti)x{ti)  +  v{ti)  (2.10) 

where  z(ti)  is  the  sensor  measurement,  H{ti)  is  the  measurement  matrix  mapping 
the  sensor’s  observation  to  the  hlter  states,  x{ti)  is  the  current  state  estimate,  and 
v{ti)  is  noise  of  sensor  model.  Similar  to  the  vehicle  model,  the  sensor  noise  describes 
the  uncertainty  of  the  sensor  model.  The  vehicle  dynamics  model  is  used  during  time 
propagation  which  is  presented  in  the  next  section. 

2. 3. 1.2  Time  Propagation.  Time  propagation  moves  the  hlter  state 
estimate  and  hlter  uncertainty  through  time  based  on  the  vehicle  dynamics  model  [16]. 
The  time  index  tj'  is  dehned  as  immediately  before  time  propagation  and  is  dehned 
as  immediately  after  time  propagation.  The  linear  Kalman  hlter  state  estimate  is 
initialized  with  the  starting  navigation  state  of  the  vehicle,  x{tQ)  =  x{tQ).  The 
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filter  covariance  also  known  as  the  filter  uncertainty,  is  initialized  with  the 

uncertainty  of  the  vehicle’s  initial  navigation  state.  This  step  is  composed  of  two 
equations.  Equation  (2.11)  presents  the  propagation  of  the  hlter  state  estimate 

+  Bd{ti)u{ti)  (2.11) 

where  is  the  new  state  estimate,  #(fj+i,tj)  is  the  state  transition  matrix, 

x(t^)  is  the  current  state  estimate,  Bditi)  is  the  control  input  matrix,  and  u{ti)  is  the 
control  input  vector.  The  state  transition  matrix  $(tj+i,fj)  and  control  input  matrix 
Bd{ti)  are  dehned  in  the  vehicle  dynamics  model.  The  Equation  (2.12)  presents  the 
propagation  of  the  hlter  uncertainty 

P{t~+i)  =  $(fi+i,fi)P(t+)$^((fi+i,fi)  +  Gd{ti)Qd{ti)G^{ti)  (2.12) 

where  is  the  new  hlter  uncertainty,  $(fj+i,fi)  is  the  state  transition  matrix, 

Pit'l)  is  the  current  hlter  uncertainty,  Gditi)  is  the  noise  matrix,  and  Qditi)  contains 
the  strength  of  each  noise  source.  The  variance  of  each  white,  Gaussian  noise  source 
dehned  in  the  noise  vector  Wd{ti)  is  contained  on  the  diagonal  of  the  noise  matrix 
Qdi^i)  as  dehned  in  Equation  (2.13) 


i  (2.13) 

y  0  ti  ^  tj 

where  Wditi)  is  the  noise  vector  and  Qditi)  contains  the  noise  source  strengths  on  the 
diagonal.  Note  that  all  the  oh-diagonal  terms  are  zero  because  each  noise  source  is 
independent. 

Time  propagation  occurs  regardless  of  the  availability  of  measurement  updates. 
However,  note  in  Equation  (2.12)  that  the  hlter  uncertainty  only  increases  during  time 
propagation.  Unless  sensor  measurements  are  provided,  the  Kalman  hlter  uncertainty 
in  the  hlter  states  will  grow  to  unusable  levels.  For  example,  some  precise  navigation 
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applications  require  centimeter-level  accuracy.  If  the  filter  uncertainty  grows  much 
beyond  10-20  centimeters,  the  hlter  navigation  solution  no  longer  provides  centimeter- 
level  accuracy.  The  measurement  update  process  is  presented  in  the  next  section. 

2. 3. 1.3  Measurement  Update.  The  measurement  update  step  occurs 
after  time  propagation  when  a  sensor  measurement  is  available  [16].  The  time  index 
dehnes  the  time  step  immediately  before  a  measurement  update  and  dehnes 
the  time  step  immediately  after  a  measurement  update.  A  measurement  update  is 
composed  of  three  equations.  First,  the  Kalman  gain  is  computed  in  Equation  (2.14) 

K(U)  =  (2.14) 

where  K{ti)  is  the  Kalman  gain,  is  the  current  hlter  uncertainty,  H{ti)  is 

the  measurement  matrix,  and  R{ti)  is  the  measurement  model  uncertainty.  Next,  the 
state  estimate  is  updated  with  the  sensor  observation  in  Equation  (2.15) 

^(tti)  =  +  K{ti)[z{ti)  -  H{ti)x{t-^^)]  (2.15) 

where  «(t)^i)  is  the  updated  state  estimate,  x(t~^^)  is  the  current  state  estimate, 
K{ti)  is  the  Kalman  gain,  z{ti)  is  the  sensor  measurement,  and  H{ti)  is  the  measure¬ 
ment  matrix.  Finally,  the  hlter  uncertainty  is  updated  to  rehect  the  information  added 
by  the  sensor  observation.  Equation  (2.16)  presents  the  hlter  uncertainty  update 

Pl-tf+i)  =  ^’(V+i)  -  K(U)H[t,)P(t—)  (2.16) 

where  P(t)'^^)  is  the  updated  hlter  uncertainty,  P{t~j^i)  is  the  current  hlter  uncer¬ 
tainty,  K{ti)  is  the  Kalman  gain,  and  H{ti)  is  the  measurement  matrix.  Note  that 
the  hlter  uncertainty  can  only  decrease  in  Equation  (2.16).  This  rehects  that  any 
measurement  update,  no  matter  how  poor,  contains  new  information  about  the  hlter 
states. 
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Observing  the  growth  and  change  in  filter  uncertainty  P{ti)  over  time  conveys  a 
measure  of  “observability”  and  uncertainty  the  system  sensors  and  vehicle  dynamics 
model  provide  for  each  of  the  system  states.  For  example,  the  filter  uncertainty  over 
time  will  remain  lower  for  the  position  states  in  a  Kalman  filter  where  the  system 
sensors  measure  position  directly  such  as  with  a  GPS  receiver  or  radio  range  system. 
Conversely,  the  uncertainty  will  continue  to  increase  over  time  for  the  position  states 
if  the  GPS  or  radio  range  system  ceases  to  provide  position  updates.  The  system 
dynamics  model  uncertainty  describes  how  the  Kalman  filter’s  uncertainty  of  the 
navigation  states  increase  during  periods  when  no  sensor  measurement  updates  are 
available. 

Models  of  real-world  systems  are  generally  non-linear.  As  is  evident  by  the 
name,  the  linear  Kalman  filter  accepts  only  linear  vehicle  and  sensor  models.  The 
linearization  process  to  obtain  a  linear  model  from  a  non-linear  model  removes  key 
information  describing  the  function  and  operation  of  the  vehicle  or  sensor  system. 
The  non-linear  nature  of  real-world  systems  can  cause  the  linear  Kalman  filter  states 
to  diverge,  resulting  in  erroneous  state  estimates.  The  extended  Kalman  filter  is 
presented  in  the  next  section  to  address  the  limitations  of  the  linear  Kalman  filter. 

2.3.2  Extended  Kalman  Filter.  The  non-linearities  contained  in  real-world 
vehicle  dynamics  and  sensor  models  must  be  included  in  the  Kalman  filter  to  help 
reduce  filter  divergence  [15].  The  extended  Kalman  filter  (EKF)  brings  model  non- 
linearities  into  the  estimation  process  by  re-linearizing  non-linear  vehicle  and  sensor 
models  about  the  is  current  state  estimate.  The  EKF  differs  from  the  linear  Kalman 
filter  by  estimating  the  error  in  the  filter  states  as  shown  in  Equation  (2.17) 

Sx{ti)  =  x{ti)  -  x{ti)  (2.17) 

where  5x{ti)  is  the  state  estimate  error  ,  x{ti)  is  the  true  state  vector,  and  x{ti)  is 
the  full  estimate  of  the  state  vector.  Additional  differences  between  the  linear  and 
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extended  Kalman  filters  include  propagation  of  the  full  system  state  with  the  non¬ 
linear  vehicle  dynamics  model  and  re-linearization  of  the  models  about  the  current 
state  estimate  before  performing  time  propagation  or  measurement  updates.  Through 
these  differences,  the  EKF  incorporates  non-linearities  in  the  vehicle  dynamics  and 
sensor  measurement  models.  The  hrst  section  presents  the  non-linear  vehicle  dynamics 
and  sensor  models.  The  following  sections  present  the  EKF  time  propagation  and 
measurement  update  algorithms 

2.3.2. 1  EKF  Models.  This  section  presents  the  non-linear  vehicle 
dynamics  and  sensor  models  used  in  the  EKF  [15].  These  models  provide  the  EKF 
with  prior  knowledge  of  the  vehicle  and  sensor  properties.  Equation  (2.18)  contains 
the  non-linear,  continuous-time  vehicle  dynamics  model 

x{t)  =  f[x{t),  u{t),t]  +  G{t)w{t)  (2.18) 

where  x{t)  is  the  first  derivative  of  the  state  vector,  x{t)  is  the  state  vector,  u{t)  is 
the  control  input  vector,  G{t)  is  the  measurement  noise  matrix  which  applies  each 
noise  source  dehned  in  w{t)  to  each  of  the  states.  The  noise  source  vector  describes 
the  uncertainty  of  the  vehicle  dynamics  model.  The  state  transition  function  /  is 
a  function  of  the  state  vector,  control  input,  and  time.  The  strengths  of  each  noise 
sources  are  dehned  in  the  matrix  Q  as  shown  in  Equation  (2.19). 

E[w{ti)w^{tj)]  =  i  ^  (2.19) 

[^0  ti  ^  tj 

Equation  (2.20)  presents  the  non-linear  sensor  model 

z{t)  =  h[x{t),t]  +  v{t)  (2.20) 
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where  z{t)  is  the  sensor  measurement,  x{t)  is  the  state  vector,  and  v{t)  is  a  vector  of 
white,  Gaussian  noises  with  strengths  describing  the  uncertainty  of  the  measurement 
model.  The  measurement  function  is  a  function  of  the  state  vector  and  time.  The 
strength  of  each  of  noise  contained  in  vector  v{t)  is  dehned  in  the  matrix  R  as  shown 
in  Equation  (2.21). 


„  R(ti)  ti  =  tj 

E[v{U)v^{t,)]  =  {  ^  (2.21) 

0  ti^  tj 

The  non-linear  vehicle  dynamics  and  sensor  models  must  be  linearized  before 
they  can  be  implemented  in  the  EKF.  Equation  (2.22)  presents  the  linearization  of 
the  vehicle  dynamics  model  about  the  current  full  state  vector  estimate 


F[x{t),  u{t),t] 


df[x,u{t),t] 

dx 


x=x{t) 


(2.22) 


where  x{t)  is  the  estimate  of  the  state  vector,  u{t)  is  the  control  input  vector,  x{t)  is 
the  state  vector,  and  u(t)  is  the  control  input  vector.  The  function  F[x(t),u(t),t]  is 
the  linearized  result  of  the  partial  derivatives  of  non-linear  vehicle  dynamics  function 
f[x(t),  u(t),t]  taken  with  respect  to  the  hlter  states.  As  before,  the  vehicle  dynamics 
are  a  function  of  the  state  estimate,  control  input  and  time.  To  obtain  the  state 
transition  matrix,  the  matrix  exponential  must  be  applied  as  shown  in  Equation  (2.23) 


ti,  x{tt)]  =  (2.23) 

where  ^[ti+i,ti,x(t^)]  is  the  state  transition  matrix,  F[x(t),u(t),t]  is  the  linearized 
vehicle  dynamics  model,  and  At  is  the  time  step  of  the  Kalman  hlter.  The  matrix 
exponential  must  be  re-applied  when  ever  the  linearized  vehicle  dynamics  model  is 
re-evaluated  with  a  new  state  estimate. 

Equation  (2.24)  displays  the  linearization  of  the  sensor  model  about  the  current 
full  state  vector  estimate 
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=  (2.24) 

where  x{t~j^i)  is  the  discrete-time  state  estimate  immediately  before  a  measurement 
update.  The  function  H[x{t~j^^),tiJ^\\  is  the  linearized  result  of  the  partial  derivatives 
of  the  non-linear  measurement  function  tj+i]  with  respect  to  the  hlter  states. 

As  stated  previously,  the  measurement  function  is  dependent  on  the  state  estimate 
and  time.  Both  the  non-linear  and  linearized  vehicle  dynamics  models  are  used  in 
time  propagation,  presented  in  the  next  section. 

2. 3. 2. 2  EKF  TimePropagation.  EKF  time  propagation  differs  from 
the  linear  Kalman  hlter  by  propagating  the  state  estimate  with  the  non-linear  vehicle 
dynamics  model  [15].  The  linearized  vehicle  dynamics  model  is  used  to  propagate 
the  error  of  the  state  estimate.  The  time  index  right  before  time  propagation  occurs 
is  denoted  tf  and  right  after  time  propagation  occurs  is  denoted  The  starting 
vehicle  navigation  states  a?(to)  initializes  EKF  full  state  estimate  x{tQ).  Likewise,  the 
uncertainty  in  the  starting  vehicle  navigation  states  initializes  the  hlter  covariance 
or  uncertaintyP(t[|').  Note  that  the  state  estimate  error  is  zero,  Sx{tQ)  =  0,  at  the 
beginning  of  hlter  operation. 

Time  propagation  for  the  EKF  consists  of  two  equations.  Figure  2.3  illustrates 
the  time  propagation  of  the  full  state  estimate  using  the  non-linear  vehicle  dynamics 
model.  Equation  (2.25)  presents  the  integration  in  discrete-time,  mathematical  form 

x{t;+i)  =  x{tf)  +  [  f[x{t),u{t),t]dt  (2.25) 

Jti 

where  x{t~[j^^)  is  the  new  state  estimate,  x{tf)  is  the  current  state  estimate,  and 
the  integration  of  the  non-linear  vehicle  dynamics  model  f[x{t),  u{t),t]  computes  the 
change  in  the  full  state  estimate. 


18 


u(ti) 

- ^ 

/(•/) 

1 

r 

^(ti) 

J 

1 

— — > 

_ < 

j 

V 

x(to) 


Figure  2.3:  Block  Diagram  for  Nonlinear  Integration.  The  full  state  estimate  x  and 
control  input  u  are  used  to  calculate  the  change  in  the  full  state  estimate  denoted  as 
X.  The  integration  step  accumulates  the  changes  to  produce  a  whole  state  estimate. 

As  stated  at  the  beginning  of  this  section,  the  initial  state  estimate  error  Sx{ti) 
is  zero.  In  the  measurement  update  step,  the  estimate  of  the  state  error  is  used  to 
correct  the  full  state  estimate,  after  which  the  state  estimate  error  is  again  returned 
to  zero.  Time  propagation  of  the  state  estimate  error  consists  of  propagating  a  vector 
of  zeros.  The  hlter  uncertainty  is  propagated  as  shown  in  Equation  (2.26) 

(2.26) 

where  P(t~^i)  is  the  new  hlter  uncertainty,  x(t~^-^)]  is  the  state  transition 

matrix  evaluated  at  the  current  full  state  estimate  x(t~^-^^),  and  P{tf)  is  the  current 
uncertainty  of  the  vehicle  dynamics  model.  Equation  (2.27)  from  [16]  is  used  to  obtain 
the  discrete-time  vehicle  model  uncertainty 


(2.27) 

where  ^>(ti,tj_i)  is  the  state  transition  matrix,  G(tj_i)  is  the  measurement  noise  ma¬ 
trix,  is  the  matrix  of  continuous-time  noise  strengths,  and  At  is  the  sample 

rate  of  the  discrete-time  Kalman  hlter.  The  next  section  presents  the  EKE  measure¬ 
ment  update. 
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2. 3. 2. 3  EKF  Measurement  Update.  The  measurement  update  step 
occurs  after  time  propagation  when  a  sensor  measurement  is  available  [15].  The  time 
index  right  before  a  measurement  update  is  denoted  and  right  after  a  measurement 
update  is  denoted  The  EKF  measurement  update  consists  of  four  equations.  As 
with  the  linear  Kalman  hlter,  the  Kalman  gain  is  computed  hrst  as  shown  in  equation 


(2.28) 

where  K{ti)  is  the  Kalman  gain,  P{t~j^i)  is  the  current  hlter  uncertainty,  tj] 

is  the  measurement  matrix,  and  R{ti)  is  the  measurement  model  uncertainty.  Next, 
the  state  error  is  computed  as  shown  in  equation 


dx{tt+i)  =  +  K{ti+i)  [z{ti+i)  -  H[x{ti^^)Ui+i]  -  H[x{ti^^)Ui+i]dx{t^^^)\ 

(2.29) 

where  is  the  new  state  estimate  error  ,  Sx(t~^^)  is  the  current  state  es¬ 

timate  error,  K{ti^i)  is  the  Kalman  gain,  z(tj_|_i)  is  the  sensor  measurement,  and 
H  [^(ti+i),ti+i\  is  the  measurement  matrix  evaluated  at  the  current  full  state  esti¬ 
mate  x{t~^i).  Since  the  state  estimate  error  is  zero  coming  into  the  measurement 
update  step.  Equation  (2.29)  may  be  simplihed  as  shown  in  Equation  (2.30). 

dx{tt+i)  =  K{ti+i)  [z{ti+i)  -  H[x{t-^^),ti+i]]  (2.30) 

The  hlter  uncertainty  update  follows  as  displayed  in  Equation  (2.31) 

P{tt+i)  =  P{t~+i)  -  K{ti+i)H[x{t-^^),ti+i]P{t-+^)  (2.31) 
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where  Pitfj^i)  is  the  new  hlter  uncertainty,  is  the  current  hlter  uncertainty, 

K{ti+i)  is  the  Kalman  gain,  and  is  the  measurement  matrix  evaluated 

at  the  current  full  state  estimate  Finally,  the  state  estimate  error  is  used  to 

correct  the  full  state  estimate  as  presented  in  Equation  (2.32) 

*(t++i)  =  +  5x{tt+i)  (2.32) 

where  xit'lj^^)  is  the  new  full  state  estimate,  x{t~j^^)  is  the  current  full  state  estimate, 
and  5x{t'lj^^  is  the  hlter  error  estimate.  After  the  full  state  estimate  is  corrected,  the 
state  estimate  error  is  reset  back  to  zero.  The  EKF  time  propagation  step  occurs  next 
and  repeats  until  another  sensor  measurement  becomes  available.  This  concludes  the 
EKF  algorithm. 

2.4  Inertial  Navigation 

The  inertial  navigation  system  (INS)  is  a  passive  navigation  device.  Unlike 
GPS,  INS  cannot  be  jammed  by  radio  frequency  signals.  The  passive  nature  of  INS 
makes  it  ideal  for  providing  navigation  capability  in  GPS-denied  environments  [25]. 

This  section  covers  strap-down  inertial  navigation  [25].  In  a  strap-down  INS 
setup  the  sensors  are  hxed  in  orientation  relative  to  the  INS  body.  This  differs  from  a 
platform  INS  where  the  sensors  are  free  to  rotate  relative  to  the  INS  body  and  remain 
stationary  relative  to  the  hxed  stars.  The  INS  accelerometer  and  gyroscope  sensors 
are  covered  in  the  hrst  section.  The  next  section  presents  the  equations  necessary  for 
strap-down  INS  mechanization.  The  last  section  details  error  sources  which  result  in 
drift  of  the  INS  navigation  solution. 

2.4-1  INS  Sensors.  The  INS  is  composed  of  two  types  of  sensors:  accelerom¬ 
eters  and  gyroscopes  [25].  Accelerometers  consist  of  a  mass  and  transducer  which, 
when  accelerated  along  its  input  axis,  generates  an  electrical  signal.  This  electrical 
signal  is  used  by  the  INS  as  the  magnitude  of  acceleration  along  the  specihed  axis. 
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This  interaction  follows  Newton’s  second  law  of  motion  which  is  described  by  the 
equation 


F  =  ma  (2.33) 

where  F  is  the  force  which  results  when  an  acceleration  a  is  applied  to  a  rigid  object 
of  mass  m.  Conversion  of  the  acceleration  into  an  electrical  signal  is  performed  by  the 
transducer.  One  form  of  transducer  measures  the  displacement  of  a  mass  attached  to 
a  spring.  As  the  acceleration  increases  along  the  input  axis,  the  displacement  of  the 
mass  increases. 

Gyroscopes  originally  consisted  of  a  spinning  mass  suspended  within  two  in¬ 
dependent  gimbals.  The  model  HG1700  INS  applied  in  this  thesis  uses  ring-laser 
gyros  which  are  described  in  this  paragraph  [19].  The  ring-laser  gyro  is  one  example 
of  an  optical  gyroscope.  Optical  gyroscopes  used  interferometers  or  interferometric 
methods  to  sense  angular  motion  [25]. 

Current  technology  has  advanced  to  use  ring  laser  gyros  which  are  described 
in  this  paragraph.  The  inertia  of  a  spinning  mass  causes  it  to  resist  changes  in 
orientation.  In  a  platform  INS,  the  gyroscope  is  not  allowed  to  process  or  rotate 
beyond  very  small  angles.  This  type  of  gyroscope  is  called  a  rate-gyro.  Force  is 
applied  by  the  spinning  mass  to  the  gimbals  when  an  angular  velocity  is  applied  to 
the  input  axis  of  the  gyroscope.  Electronic  transducers  on  the  gimbals  measure  the 
force  applied  to  the  gimbals  by  the  spinning  mass,  allowing  measurements  of  angular 
rate. 

An  INS  typically  hosts  three  accelerometers  and  three  gyroscopes.  The  input 
axes  of  the  accelerometers  and  gyroscopes  are  positioned  at  right-angles  along  the 
three  main  axes  of  the  INS  6-frame.  The  accelerometers  measure  specihc  force  which 
accounts  for  both  changes  in  velocity  and  gravity.  The  gyroscopes  measure  the  rate  of 
angular  rotation  of  the  INS  platform.  Together,  these  six  sensors  enable  measurement 
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of  the  position,  velocity,  and  attitude  of  the  INS  platform  through  the  mechanization 
of  the  sensor  data. 

2.4-2  INS  Mechanization.  The  data  provided  by  the  accelerometers  and 
rate-gyros  is  mechanized  through  navigation  equations  to  provide  measurements  of 
position,  velocity,  and  attitude  of  the  INS  6-frame  [25].  The  strap-down  nature  of  the 
INS  requires  knowledge  of  the  INS  orientation  for  correct  usage  of  the  accelerometer 
measurements. 

INS  mechanization  begins  with  attitude  calculations  based  on  gyroscope  mea¬ 
surements.  The  gyroscopes  measure  the  angular  rate  of  the  INS  body.  Equation  (2.34) 
shows  the  calculation  of  the  INS  body  rate 

=  +  (2.34) 

where  is  the  body  rate,  is  the  angular  rates  measured  by  the  INS  gyroscopes, 
is  the  DCM  from  the  INS  6-frame  to  the  local  n’-frame,  is  the  turn  rate  of 
the  Earth  with  respect  to  the  i-frame  expressed  in  the  n’-frame,  and  a;”)),  is  the  turn 
rate  of  the  local  n’-frame  with  respect  to  the  e-frame,  expressed  in  the  n’-frame. 

The  orientation  of  the  INS  body  resides  in  the  DCM  C[(,.  This  DCM  is  updated 
with  the  INS  body  rate  through  Equation  (2.35) 

cl  =  (2.35) 

where  is  the  skew-symmetric  form  of  the  body  rate.  Equation  (2.36)  displays  the 
skew-symmetric  form 


0  —Uz  COy 

Uz  0  —u, 

OJy  UJx  0 


(2.36) 
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where 

I  uT 

Next,  the  INS  body  DCM  is  transposed,  =  C^, ,  to  rotate  the  accelerometer 
specihc  force  measurements,  =  [fx-,fy,fz],  from  the  INS  6-frame  to  the  local  n’- 
frame.  Equation  (2.37)  presents  the  computation  of  change  in  velocity 

<  =  Ct'f  -  [2wt  +  wt]  X  +  gf  (2.37) 

where  is  the  change  in  velocity  with  respect  to  the  e-frame  expressed  in  the  local 
n’-frame,  (7^  is  the  INS  body  orientation,  is  the  accelerometer  measurements 
in  the  INS  6-frame,  a;”'  is  the  turn  rate  of  the  Earth  with  respect  to  the  i-frame 
expressed  in  the  n’-frame,  is  the  turn  rate  of  the  local  n’-frame  with  respect  to 
the  e-frame  expressed  in  the  n’-frame,  =  [vn,ve-,vdY  is  the  velocity  with  respect 
to  the  e-frame  expressed  in  the  local  n’-frame,  and  g"^'  contains  the  local  gravity 
vector. 

The  INS  mechanization  contained  in  the  above  equations  integrates  the  changes 
in  velocity  and  body  orientation  to  compute  the  velocity  and  attitude  of  the  INS  body 
in  the  local  n’-frame.  Integration  of  the  velocity  provides  navigation  position 
relative  to  the  starting  location  of  the  INS. 

2.4-3  INS  Error  Sources.  Imperfections  in  accelerometers,  gyroscopes,  and 
computation  systems  are  three  areas  that  contribute  to  accumulation  of  error  in  the 
INS  position,  velocity  and  attitude  solution  [25].  This  accumulation  of  error  appears 
as  a  drift  in  the  INS  navigation  solution.  Additional  sources  of  error  arise  from  un¬ 
certainty  in  the  initial  INS  position  and  orientation.  The  INS  provides  navigation 
relative  to  a  starting  location  and  attitude.  Any  uncertainty  in  the  initial  position 
or  attitude  introduces  error  into  the  INS  navigation  solution.  The  following  sections 
present  details  for  errors  contributed  by  the  accelerometers,  gyroscopes,  and  compu¬ 
tation  system. 
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2. 4- 3.1  Accelerometer  Errors.  Accelerometers  have  four  main  sources 
of  error  which  contribute  to  INS  navigation  drift  [25].  First,  a  hxed  bias  error  is  an 
offset  present  in  the  measured  acceleration  which  remains  constant  regardless  of  the 
application  of  further  acceleration.  Next,  a  scale-factor  error  is  a  ratio  which  describes 
the  error  between  the  applied  and  the  measured  acceleration.  Cross-coupling  errors 
are  erroneous  outputs  from  the  accelerometer  to  accelerations  applied  orthogonal  to 
its  input  axis.  Finally,  vibro-pendulous  errors  describe  the  resonance  characteristics 
of  the  suspended  mass  inside  an  accelerometer.  These  error  sources  are  taken  into 
account  in  the  accelerometer  model  shown  in  Equation  (2.38) 

Q-x  T  Sx)^x  “t“  “1“  Bj  -\-  B^OxOiy  “t“  Tlx  (2.38) 

where  Ox  is  the  corrupted  accelerometer  measurement,  Ox  is  the  true  accelerometer 
measurement,  Sx  is  the  scale  factor  error.  My  and  Mz  are  the  cross-axis  coupling 
errors,  Bf  is  the  measurement  bias,  B^  is  the  vibro-pendulous  error  coefficient,  and 
Ux  is  a  random  bias  which  is  caused  by  instabilities  within  the  accelerometer  assembly. 

2. 4- 3. 2  Gyroscope  Errors.  The  rate-gyroscopes  used  in  the  INS  have 
six  signihcant  sources  of  error  [25].  The  hrst  three  error  sources  describe  non-idealities 
in  the  structure  of  the  gyroscope.  Reduction  in  gyroscope  sensitivity  due  to  flex  lead 
torques  and  friction  in  the  gimbal  pivots  is  termed  g-insensitivity  bias.  Increased 
gyroscope  sensitivity  due  to  unbalanced  mass  on  the  gimbals  or  unbalance  spinning 
gyro  mass  is  termed  g-sensitivity  bias.  Third,  the  anisoelastic  bias  describes  unbal¬ 
anced  compliance  of  the  gyroscope’s  float  assembly  along  the  input  and  spin  axes. 
The  remaining  three  error  sources  are  the  scale-factor  error,  cross-coupling  error,  and 
the  zero-mean  random  bias  error. 

Similar  to  the  accelerometer,  the  scale-factor  error  describes  the  ratio  of  applied 
angular  velocity  to  the  measured  angular  velocity.  Cross-coupling  errors  account  for 
the  effects  of  angular  velocities  applied  orthogonal  to  the  gyro’s  input  axis.  Finally,  the 
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zero-mean  random  bias  captures  errors  arising  from  gyroscope  mechanical  instabilities 
such  as  pivot  friction  and  random  movements  of  the  spin  motor  about  the  gyro’s  spin- 
axis.  Equation  (2.39)  presents  a  formula  which  accounts  for  the  effects  of  these  errors 
on  the  output  signal  from  a  gyroscope 


—  (1  +  Sx)uJx  +  +  -B/x  +  Bg^dx  +  Bg^dz  +  B  axz^x^z  +  “dx  (2.39) 

where  Ux  is  the  measured  angular  rate  of  the  gyro  about  its  input  axis;  Ux  is  the  true 
angular  rate  of  the  gyro  about  its  input  axis;  Uy  and  Uz  are  the  angular  rates  about 
the  output  and  spin  axes  of  the  gyro,  respectively;  dx  and  dz  are  the  accelerations  of 
the  gyro  along  the  input  and  spin  axes,  respectively.  Sx  is  the  scale  factor.  My  and 
Mz  are  the  cross-coupling  coefficients,  Bfx  is  the  g-insensitivity  bias,  Bgx  and  Bgz  are 
the  g-sensitivity  bias  coefficients,  Baxz  is  the  anisoelastic  bias  coefficient  and  Ux  is  the 
zero-mean  random  bias. 

2.4-4  Computation  Errors.  Measurements  from  the  accelerometers  and 
gyroscopes  are  applied  to  the  mechanization  equations  shown  in  Section  2.4.2  by 
computers.  Limitations  in  computers  comprise  the  third  set  of  errors. 

Computers  have  fixed  precision  due  to  pre-determined  register  sizes  and  memory 
limitations.  Numerical  truncation  due  to  limited  precision  adds  error  to  each  INS 
computation.  Bandwidth  limitations  due  to  the  internal  clock  frequency  of  the  INS 
computer  present  additional  error  where  changes  in  attitude  and  velocity  occur  faster 
than  the  computer  is  sampling.  These  measurements  are  lost,  resulting  in  unaccounted 
attitude  and  velocity  changes  which  add  to  the  INS  error. 

2.5  Radio  Positioning  Methods 

The  errors  accumulated  from  navigation  with  an  INS  presented  in  Section  2.4 
grow  over  time  at  an  increasing  rate  which  is  seen  as  a  drift  in  the  navigation  solution. 
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To  obtain  precise  navigation  solutions,  the  drift  present  in  INS  navigation  systems 
must  be  constrained.  A  source  of  error  constraint  comes  from  sensors  with  non- 
integrated  measurement  error.  Navigation  by  radio  range  measurements  is  presented 
as  a  source  of  non-integrated  error. 

This  section  proceeds  with  a  survey  of  other  work  conducted  in  the  area  of  range- 
aided  navigation.  Next,  three  techniques  for  positioning  a  mobile  radio  transceiver 
relative  to  stationary  radio  transceivers  with  known  locations.  The  mobile  radio 
transceiver  is  labeled  as  a  Mobile  Station  (MS)  and  the  stationary  radio  transceiver 
is  labeled  as  a  Base  Station  station  (BS).  Radio  frequency  (RF)  signals  sent  between 
the  MS  and  BSs  provide  positioning  information.  Time  of  Arrival  (TOA),  Angle  of 
Arrival  (AOA),  and  Received  Signal  Strength  (RSS)  comprise  general  forms  of  radio 
positioning  techniques  [14] . 

2.5.1  Range- Aided  Navigation  Survey.  This  section  presents  an  survey  of 
other  research  that  has  been  conducted  in  range-aided  navigation  technologies.  Ap¬ 
plications  range  from  military  vehicle  navigation  to  industrial  asset  tracking  and  hre- 
hghter  personnel  accountability. 

Ernsberger  [8]  explored  radio-aided  navigation  with  the  incorporation  of  image, 
baro-altimeter,  and  inertial  measurements.  Through  computer-based  simulations, 
the  performance  of  this  extended  Kalman  hlter-based  navigation  system  was  evalu¬ 
ated.  The  simulated  range  system  consisted  of  radios  fixed  to  the  ground  and  radios 
mounted  on  moving  vehicles.  The  imaging  system  consisted  of  monocular  sensors 
mounted  to  each  of  the  vehicles.  The  horizontal  RMS  navigation  position  error  us¬ 
ing  only  range  and  baro-altimeter  measurements  was  20.24  meters.  Incorporation  of 
the  imaging  system  and  inertial  measurements  produced  a  horizontal  RMS  position 
error  of  6.81  meters.  The  inertial  and  image  data  provided  a  notable  improvement  in 
positioning  accuracy. 

Ultra  wide  band  (UWB)  positioning  technology  also  presents  a  viable  range- 
aided  navigation  system.  Fontana,  Richley,  and  Barney  [10]  detail  an  UWB  posi- 
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tioning  system  use  to  position  and  track  assets.  The  specific  application  is  industrial 
facilities  and  hospitals.  This  UWB  ranging  system  uses  a  network  of  receivers  placed 
at  know  locations  inside  the  building.  Then,  small  active  UWB  tags  are  placed  on 
objects  to  be  positioned  and  tracked.  The  burst  transmission  from  each  active  tag 
includes  an  ID  and  optional  fields  for  additional  data.  The  interval  of  tag  transmis¬ 
sion  can  range  from  5  seconds  to  an  hour  depending  on  the  application.  Battery 
life  in  excess  of  3.8  years  can  be  obtained  from  the  1  amp-hour  lithium  battery  cells 
contained  in  each  active  tag.  A  central  processing  hub  consisting  of  a  dedicated  com¬ 
puter  processes  the  time-tagged  signals  collected  from  each  active  tag.  Time  of  arrival 
methods  are  used  with  leading-edge  signal  detection  circuitry  to  obtain  sub-foot  po¬ 
sition  accuracy.  Positioning  standard  deviation  below  0.5  feet  is  demonstrated  in  this 
paper. 

Additional  range-aided  navigation  applications  are  found  in  the  arena  of  search 
and  rescue.  Worcester  Polytechnic  Institute’s  (WPI)  [4-6]  Precision  Personnel  Loca¬ 
tor  (PPL)  system  is  one  such  research  effort  which  has  produced  a  radio-range  based 
positioning  system  which  tracks  personnel  in  indoor  environments  assuming  no  exist¬ 
ing  radio  infrastructure.  This  positioning  system  consists  of  reference  radio  stations 
potentially  located  on  emergency  vehicles.  Once  the  vehicles  arrive  at  a  scene,  the 
PPL  system  performs  an  auto-calibration  to  position  each  of  the  emergency  vehicles 
and  setup  a  local  navigation  reference  frame.  Small  transmitters  worn  by  each  of  the 
emergency  personnel  transmit  time-tagged  data  which  is  received  by  in-range  refer¬ 
ence  stations.  Time-difference  of  arrival  (TDOA)  algorithms  are  used  to  position  each 
of  the  personnel  within  range  of  this  system.  Additional  relevant  information  such 
as  distress  signals,  temperature,  and  personnel  diagnostic  data  is  also  relayed  to  the 
reference  stations.  A  base  station  collects  the  data  from  the  reference  stations  and 
provides  a  user-interface  which  presents  relevant  position  and  health  information  to 
the  command  and  control  center  at  the  scene. 

Key  contributions  from  each  of  these  research  efforts  include  simulation  of  range- 
aided  navigation  with  additional  sensors,  implementation  of  range-aided  positioning 


in  challenging  indoor  environments,  and  range-aided  navigation  systems  which  do  not 
require  prior  information  of  the  building  or  environment.  This  section  continues  with 
presentation  of  range-aiding  navigation  algorithms  and  techniques. 

2.5.2  Time  of  Arrival.  TOA  measurements  provide  slant  range  distances  be¬ 
tween  two  radios.  The  TOA  radio  positioning  method  is  implemented  by  the  Raytheon 
DH500  radios  used  in  this  thesis.  Slant  range  is  dehned  as  the  line  of  sight  (LOS) 
distance  between  the  BS  and  MS.  The  slant  range  is  directly  proportional  to  the 
propagation  time  of  the  radio  signal  from  the  BS  to  the  MS.  Equation  (2.40)  presents 
the  calculation  of  the  slant  range 


Ri  =  c(tMS  —  tBSi)  (2.40) 

where  Ri  is  the  slant  range,  tsSi  time  the  signal  left  the  base  station,  Ims  is 

the  time  the  signal  arrived  at  the  mobile  station,  and  c  is  the  speed  of  light.  TOA 
positioning  algorithms  require  precise  time  synchronization  between  all  radios.  TOA 
also  requires  a  time  stamp  or  symbol  be  inserted  into  the  transmitted  signal  to  allow 
the  receiving  radio  to  determine  when  the  signal  left  the  transmitter. 

RF  signal  multipath  signihcantly  degrades  TOA  measurement  accuracy  [14]. 
Multipath  is  present  even  when  two  radios  have  LOS  visibility.  RF  signals  bounce 
off  buildings  and  other  objects  in  the  environment  arriving  at  the  receiving  radio  at 
different  times.  This  presents  a  positive  bias  or  apparent  increase  in  the  distance 
between  two  radios  for  the  reflected  RF  signals.  Specialized  algorithms  presented  in 
the  Section  2.6  address  both  the  multipath  and  non-LOS  (NLOS)  issues. 

Given  a  two-dimensional  Cartesian  coordinate  system,  a  minimum  of  two  slant 
ranges  are  required  to  solve  for  the  location  of  the  mobile  radio.  Equation  (2.41) 
present  the  mathematical  solution 


Ri  =  \/  {xmS  —  XBSiY  +  {VMS  —  VBSiY  (2-41) 
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Figure  2.4:  Positioning  Based  on  Time  of  Arrival  for  a  3-Dimensional  Solution.  The 
time  of  arrival  for  the  radio  signals  sent  between  each  mobile  and  base  station  pair 
presents  a  measurement  of  the  slant  range  distance  between  the  radio  antennas  [14]. 


Figure  2.5:  Positioning  Based  on  Angle  of  Arrival  for  a  2-Dimensional  Solution. 

Strict  dehnitions  of  radio  signal  arrival  angles  are  required  to  perform  angle  of  arrival 
radio  positioning  [14]. 

where  xms  and  hms  ai’e  the  position  of  the  mobile  station,  xsSi  and  ysSi  are  the 
position  of  the  Ah  base  station,  and  Ri  is  the  true  slant  range  between  them.  At 
least  three  slant  range  measurements  are  required  to  solve  for  the  MS  position  in  a 
three-dimensional  coordinate  system  as  shown  in  Figure  2.4. 

2.5.3  Angle  of  Arrival.  AOA  measurements  provide  the  angle  of  signal 
arrival  to  a  MS  from  a  BS.  A  beneht  of  AOA  techniques  is  the  lack  of  time  synchro¬ 
nization  needed  between  the  MS  and  BSs.  A  signihcant  drawback  is  the  specialized 
hardware  needed  to  determine  the  AOA  of  incoming  RF  signals  [14].  Figure  2.5 
presents  a  graphical  depiction  of  two-dimensional,  AOA  positioning. 
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A  two-dimensional  position  calculation  requires  two  AOA  measurements.  Equa¬ 
tions  (2.42)  and  (2.43)  present  the  two-dimensional  mathematical  position  solution 


XmS 


XBSitan{n  -  6>i)  +  XBS2tan{n  -  62)  +  yBS2  -  Vbsi 

tan{n  —  6*1)  -|-  tan{n  —  6*2) 


(2.42) 


VMS 


yBSitan{n  -  62)  +  yBS2tan{n  -  6*1)  {xbs2  ~  XMSi)tan{7i 

tan{n  —  9i)  +  tan^n  —  62) 


9i)tan{n  —  6^2) 
(2.43) 


where  xbsi  yBSi  are  the  position  of  base  station  1,  xbs2  and  yBS2  are  the  position 
of  base  station  2,  9i  is  the  angle  from  base  station  1  to  the  mobile  station,  ^2  is  the 
angle  from  base  station  2  to  the  mobile  station,  and  xms  and  yMs  is  the  resulting 
position  of  the  mobile  station.  Three  AOAs  must  be  received  to  determine  the  three- 
dimensional  location  of  the  MS. 


The  accuracy  of  AOA  positioning  depends  on  the  accuracy  of  AOA  measure¬ 
ment.  As  the  receiving  radio  moves  further  away  from  the  transmitting  radio,  AOA 
measurements  become  less  accurate.  This  is  due  to  the  geometry  of  the  radios  and 
RF  multipath  introduced  by  reflections  of  the  RF  signals  from  objects  present  in  the 
environment. 


2.5.4  Received  Signal  Strength.  The  RSS  positioning  algorithm  provides 
measurement  of  RF  signal  attenuation  between  two  radios  [14].  The  farther  a  RF  sig¬ 
nal  propagates,  the  more  the  received  signal  strength  decreases.  Given  a  RF  channel 
model  and  known  initial  signal  strength,  the  amount  of  signal  attenuation  directly 
correlates  to  the  distance  between  the  radios.  The  RSS  method  provides  accurate 
slant  ranges  for  an  environment  clear  of  multipath  fading  and  shadowing.  Benehts 
of  RSS  include  the  simplicity  in  hardware  requirements  and  lack  of  the  need  for  pre¬ 
cise  time  synchronization  between  the  MS  and  BSs.  Unknown  RF  channel  models 
present  a  signihcant  drawback  for  the  RSS  positioning  method.  Both  outdoor  and 
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Figure  2.6:  Positioning  Based  on  Received  Signal  Strength  for  3-Dimensional  So¬ 
lution.  Un- modeled  RF  channel  signal  reduction  between  each  mobile  and  base  radio 
pair  introduces  signihcant  uncertainty  into  the  position  solution  [14]. 

indoor  NLOS  conditions  present  multipath  fading  issues  with  varying  RF  channel 
models  [14].  Advanced  algorithms,  RSS  contour  maps,  and  multiple  RSS  measure¬ 
ments  from  several  stationary  radios  help  to  mitigate  these  issues  [14] .  A  minimum  of 
two  BSs  are  required  for  a  two-dimensional  position  solution  and  three  are  required  for 
a  3D  solution.  Figure  2.6  presents  a  graphical  representation  of  a  three-dimensional 
RSS  solution. 

2.6  Multipath  Mitigation 

The  radio  range  positioning  algorithms  presented  in  the  previous  section  heav¬ 
ily  depend  on  clear  RF  channels  between  radios  to  provide  accurate  measurements. 
Buildings,  walls,  and  other  obstructions  sources  signihcantly  degrade  the  position 
solution  obtained  from  each  positioning  algorithm  [27].  A  signihcant  source  of  posi¬ 
tion  error  comes  from  multipath  interference.  Estimation  and  removal  of  the  errors 
contributed  by  multipath  allows  for  calculation  of  a  much  more  accurate  position 
solution. 

This  section  covers  software-based  methods  to  estimate  and  reduce  multipath 
effects  on  TOA  measurements  between  two  radios.  The  ideal  RF  channel  between 
the  BS  and  MS  radios  is  free  of  RF  signal  rehections,  scattering,  and  fading.  This 
condition  is  rarely  achieved,  especially  in  urban  canyons  where  buildings,  electrical 
services,  and  vehicles  cause  RF  rehection,  scattering,  and  fading.  As  a  radio  wave 
bounces  off  buildings,  the  signal  strength  decreases  and  the  overall  signal  path  in- 
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Figure  2.7:  Depiction  of  RF  Multipath.  The  signal  from  the  transmitter  decreases 
in  strength  with  each  reflection.  The  reflected  signals  travel  a  longer  total  distance 
to  the  receiver  which  adds  a  positive  error  bias  to  the  slant  range  measurement. 

creases.  The  increased  signal  path  causes  the  TOA  measurements  to  increase  with  a 
positive  error  bias. 

Figure  2.7  presents  a  depiction  of  a  multipath  environment  and  NLOS  effects 
on  a  single  radio  wave.  Given  the  signal  path  between  the  MS  and  BS  is  LOS, 
part  of  the  radio  wave  will  arrive  at  the  receiver  directly  from  the  transmitter.  The 
rest  of  the  radio  wave  is  reflected,  scattered,  and  the  signal  strength  is  reduced  as 
the  number  of  signal  reflections  increases.  Also,  note  the  increase  in  signal  path 
length  from  the  strongest  LOS  path  to  the  weakest  NLOS  path.  The  increased  signal 
path  appears  as  a  positive  error  in  the  slant  range.  A  broad  array  of  both  software 
and  hardware  methods  exist  to  reduce  and  mitigate  multipath  effects.  This  section 
presents  an  overview  of  software-based  error  mitigation  techniques  which  occurs  after 
the  hardware  due  the  proprietary  nature  of  the  Raytheon  DH500  radios. 

Software  based  multipath  mitigation  methods  take  on  several  forms.  Linear  and 
Gaussian  models  such  as  the  Ring  of  Scatterers  (ROS),  Disk  of  Scatterers  (DOS),  and 
clipped  Gaussian  models  provide  well-known  methods  of  estimating  NLOS  multipath 
features  [2,3].  These  three  models  are  briefly  presented  in  this  section.  One  Kalman 
filter  multipath  mitigation  method  uses  a  biased  Kalman  filter  to  estimate  and  smooth 
the  NLOS  effects  on  the  TOA  measurements  [12].  Another  Kalman  filter  method  uti¬ 
lizes  an  interacting  multiple  model  (IMM)  Kalman  filter  with  individual  models  tuned 
for  LOS  and  NLOS  conditions.  As  the  MS  moves  in  and  out  of  NLOS  conditions. 
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the  IMM  Kalman  filter  selects  the  appropriate  model  based  on  the  incoming  signal 
variance  [7,9]. 

The  Cramer-Rao  lower  bound  (CRLB)  provides  a  mathematical  calculation  of 
the  lower  limit  for  the  covariance  matrix  of  an  unbiased  estimate  of  unknown  param¬ 
eters.  Evaluation  of  the  Kalman  hlter  uncertainty  in  terms  of  the  CRLB  provides  the 
best  expected  navigation  performance.  Given  known  multipath  conditions,  a  modi- 
hed  algorithm  termed  the  generalized  CRLB  (G-CRLB)  provides  the  best  expected 
positioning  accuracy  [21]. 

The  three  scatterer  models  mentioned  above  are  depicted  in  Figure  8(c).  These 
models  describe  the  likelihood  that  an  object  causing  scatter  is  present  in  the  specihed 
region.  The  BS  radios  are  positioned  at  known  stationary  locations.  The  MS  radio 
moves  around  within  the  region  of  application.  Application  of  these  models  to  obtain 
a  2-dimensional  position  solution  requires  at  least  three  separate,  visible  BS  radios. 
The  next  paragraph  develops  the  scatterer  models. 

In  order  to  develop  these  three  models,  several  variables  are  dehned.  The  error 
in  the  measured  slant  range  r]ij  is  computed  in  Equation  (2.44) 

Vij  ~  hj  ~  Ri  (2.44) 

where  Ri  is  the  true  slant  range  from  BS*  to  the  MS  in  the  center  of  the  models  and 
the  j-th  multipath  corrupted  slant  range  measured  by  the  MS  is  labeled  Uj.  Imple¬ 
mentation  of  each  model  consists  of  statistical  estimation  of  the  mean  and  variance 
of  the  incoming  slant  range  measurements.  These  statistics  are  used  to  iteratively 
estimate  the  MS  position  (xms,yms)  and  associated  model  parameter  r. 

The  dehnition  of  the  model  parameter  r  depends  on  the  model  implemented. 
For  the  ROS  model,  r  =  where  R^  dehnes  the  radius  of  the  ring.  For  the  DOS 
model,  r  =  Rd  where  Rd  defines  the  radius  of  the  disk.  For  the  Gaussian  model, 
r  =  ag  where  ag  dehnes  the  standard  deviation  of  the  Gaussian  scatterer  distribution 
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(c)  Clipped  Gaussian  Scatter  Model 


Figure  2.8:  Illustration  of  the  Three  Scatterer  Models  [3].  The  scatterers  he  on 

the  ring  for  the  ROS.  For  the  DOS  model,  the  scatterers  are  uniformly  distributed 
within  the  ring.  The  clipped  Gaussian  model  positions  the  scatterers  in  a  Gaussian 
distribution  centered  around  the  master  station. 


35 


on  the  disk.  The  three  model  parameters  are  defined  for  simplicity  in  Equation  (2.45). 
Definition  of  the  PDFs  for  each  model  is  contained  in  [2]. 


Rr 

ROS  Model 

Rd 

DOS  Model 

(2.45) 

^9 

Gaussian  Clipped  Model 

Application  of  the  scatterer  models  to  determine  the  position  (xms,yms)  of  the 
MS  consists  of  computing  the  variance  of  the  TOAs  for  at  least  three  BS-MS  pair. 
Next,  instantiate  the  PDF  for  the  desired  scatterer  model  three  times,  equating  the 
variances  from  each  radio  pairs  to  each  of  the  PDF  realizations.  The  position  of  the 
MS  is  found  through  simultaneous  solution  of  the  three  PDF  equations  [2]. 

Al-Jazzar  and  Caffery  [2, 3]  present  results  which  show  the  error  reduction  ca¬ 
pability  of  the  scatterer  models.  Position  errors  are  reduced  by  100-200  meters  de¬ 
pending  on  the  model  used  and  associated  parameters.  The  results  presented  also 
show  the  scatterer  models  to  be  effective  at  position  error  reduction  when  applied  to 
environments  which  do  not  conform  to  these  models. 

2.7  Baro- Altitude  Aiding 

Radio  range  positioning  on  the  surface  of  the  Earth  provides  excellent  observ¬ 
ability  of  the  horizontal  navigation  channels,  but  poor  observability  in  the  vertical 
navigation  channel.  A  barometer  provides  measurement  of  air  pressure.  Since  air 
pressure  varies  with  altitude,  barometric  pressure  provides  an  indirect  measurement 
of  altitude.  The  use  of  barometric  pressure  to  aid  in  altitude  measurement  is  termed 
baro-altitude  aiding.  Baro-altitude  aiding  is  presented  in  this  section  to  aid  in  mea¬ 
surement  of  the  vertical  navigation  channel.  This  section  presents  an  air  pressure  to 
altitude  conversion  formula  and  then  discusses  errors  associated  with  baro-altitude 
aiding. 
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Baro-altitude  aiding  provides  a  relative  measure  of  altitude  based  on  current  air 
pressure.  As  altitude  increases,  air  pressure  decreases  and  vise  versa.  The  difference 
in  air  pressure  between  two  locations  provides  a  measure  of  altitude  change.  Equa¬ 
tion  (2.46)  presents  the  conversion  from  change  in  air  pressure  to  change  in  height  [24] 

Ah  =  44330  ■  1^1  -  j  -  hinit  (2.46) 

where  Ah  is  the  change  in  height  in  meters,  P  is  the  current  air  pressure  in  kPa,  Pq  is 
defined  as  101.325  kPa  which  is  the  standard  pressure,  and  hinu  is  the  initial  height 
in  meters. 

Several  error  sources  exist  for  baro-altitude  aiding.  Local  weather  phenomenon 
causes  air  pressure  to  change  slowly  over  time.  The  use  of  baro-altitude  aiding  in¬ 
side  buildings  or  vehicles  introduces  further  error  from  building  pressurization  by 
the  ventilation  system  and  pressure  changes  due  to  opening  and  closing  doors.  A 
stationary  reference  barometer  provides  mitigation  of  air  pressure  drift  due  to  local 
weather  phenomenon  [24].  Careful  Kalman  filter  barometer  measurement  integration 
and  barometer  response  modeling  can  help  mitigate  error  effects  from  ventilation  sys¬ 
tems  and  operation  of  building  doors.  The  next  paragraph  overviews  a  Kalman  filter 
implementation  of  baro-altitude  aiding. 

The  EKF  implemented  in  this  thesis  uses  baro-altitude  aiding  to  assist  in  vertical 
channel  measurement.  Correct  EKF  implementation  requires  modeling  of  the  error 
source  present  in  the  baro-altitude  aiding  sensor.  Kim  and  Sukkarieh  [11]  model  the 
barometric  error  as  the  output  of  a  first-order  Gauss  Markov  (FOGM)  process  with  the 
measurement  noise  modeled  by  a  white  Gaussian  noise  source.  Figure  2.9  presents 
the  block  diagram  depiction  of  the  FOGM  generation  process.  The  period  /3  and 
magnitude  ^J2a‘^l3  of  the  FOGM  process  are  defined  by  analyzing  real  baro-altimeter 
data  [11].  The  magnitude  of  the  white  Gaussian  measurement  process  noise  v{t)  is 
determined  based  on  the  individual  sensor  characteristics.  The  Gaussian,  zero-mean 
noise  sources  w{t)  and  v{t)  are  independent. 
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Figure  2.9:  Barometer  Error  Model.  The  FOGM  describes  barometer  error  and  the 
white  Gaussian  noise  source  v{t)  describes  measurement  process  noise. 

2.8  Chi  Square  Statistics 

Sensor  models  used  in  linear  and  extended  Kalman  hlters  assume  the  uncer¬ 
tainty  or  error  distribution  of  sensor  is  Gaussian  [16].  To  ascertain  whether  the 
measurement  errors  from  a  given  sensor  £t  a  Gaussian  distribution,  the  Ghi  Squared 
statistical  test  is  introduced.  The  Ghi  Squared  test  provides  a  numerical  Goodness 
Of  Fit  (GOF)  assessment  of  how  well  a  collection  data  points  conforms  to  a  null 
hypothesis  [13].  For  the  purposes  of  this  thesis,  the  null  hypothesis  is  the  Gaussian 
distribution.  Explanation  of  the  Ghi  Squared  algorithm  follows. 

The  sample  space  of  the  given  data  set  of  sensor  error  and  of  the  null  hypothesis 
Sx  is  divided  up  into  K  disjoint  intervals.  The  number  of  sensor  error  samples 
contained  in  each  interval  is  labeled  Uk-  Likewise,  the  number  of  null  hypothesis 
samples  contained  in  each  interval  is  represented  by  ruk-  Next,  apply  the  formula 
provided  in  Equation  (2.47)  to  compute  the  Ghi  Squared  statistic 


(ufc  -  mkf 
rrik 


(2.47) 


where  is  the  Ghi  Squared  statistic,  K  is  the  number  of  disjoint  intervals,  Uk 
is  the  number  of  error  samples  within  each  interval,  and  ruk  is  the  number  of  null 
hypothesis  within  each  interval.  The  value  is  low  if  the  range  error  fits  the  null 
hypothesis.  If  the  range  error  does  not  fit  the  null  hypothesis,  has  a  high  value. 
The  outcome  of  this  comparison  is  presented  as  non- Gaussian  or  Gaussian,  either 
rejecting  or  accepting  the  null  hypothesis  respectively. 
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The  threshold  comparison  of  is  performed  at  the  5%  signihcance  level.  The 
signihcance  level  is  represented  by  the  variable  ta-  Table  8.3  in  [13]  contains  a  set 
of  values  for  various  signihcance  levels.  Therefore,  if  >  ta  the  outcome  is  non- 
Gaussian  and  the  null  hypothesis  is  rejected.  If  D'^  <  ta  the  outcome  is  Gaussian 
and  the  null  hypothesis  is  accepted. 

The  Chi  Squared  test  provides  a  numerical  assessment  of  how  well  the  measure¬ 
ments  from  a  sensor  £t  a  white  Gaussian  distribution.  The  Chi  Squared  GOF  test 
is  used  in  Chapter  IV  to  provide  a  mathematical  determination  whether  a  data  set 
fits  the  Gaussian  distribution  or  not.  The  Chi  Squared  test  assists  in  evaluation  and 
refinement  of  sensor  models  used  in  the  Kalman  filter  where  the  measurement  uncer¬ 
tainty  is  assumed  to  be  white  and  Gaussian  in  its  distribution.  However,  development 
of  accurate  sensor  models  when  errors  do  not  conform  to  the  white  Gaussian  distri¬ 
bution  assumption  can  be  prohibitively  complex.  The  next  section  presents  adaptive 
filters  which  provide  an  alternative  to  complex  sensor  models. 

2.9  Adaptive  Filter  Algorithms 

The  Kalman  hlter  models  sensor  error  with  predetermined  measurement  uncer¬ 
tainty  contained  in  the  variable  R.  The  basic  Kalman  hlter  or  EKF  assumes  the  error 
contained  in  the  sensor’s  measurements  remains  constant  during  the  Kalman  hlter’s 
operation.  Radio  range  measurements  break  this  assumption  in  real-world  operation. 
The  results  of  range  characterization  presented  in  Chapter  IV  will  clearly  show  the 
actual  range  measurement  error  is  significantly  greater  compared  to  the  predefined 
measurement  uncertainty  R{ti).  The  discrepancy  between  the  predetermined  uncer¬ 
tainty  and  actual  range  uncertainty  causes  the  Kalman  hlter  to  improperly  use  the 
range  measurements.  Adaptive  hlter  algorithms  allow  the  measurement  uncertainty 
to  be  adjusted  during  hlter  operation  which  enables  the  Kalman  hlter  to  properly 
utilize  the  range  measurements. 

The  two  adaptive  hlter  algorithms  discussed  in  this  section  modify  the  hlter 
techniques  presented  in  Section  2.3  to  account  for  changes  the  system  dynamics  and 
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sensor  model  uncertainties.  Residual  monitoring  presents  a  technique  to  discard  mea¬ 
surements  with  uncertainty  greater  than  a  predetermined  threshold.  Presentation  of 
Sage-Husa  algorithm  follows  which  contains  a  notable  computational  efficiency  im¬ 
provement  over  the  AKF  and  adds  estimation  of  the  mean  of  the  system  dynamics 
and  measurement  model  uncertainty. 

2.9.1  Residual  Monitoring.  Residual  monitoring  encompasses  a  group  of  al¬ 
gorithms  which  provide  information  based  on  the  measurement  residual.  Applications 
of  residual  monitoring  include,  sensor  failure  detection  by  application  of  a  likelihood 
function  and  rejection  of  spurious  measurements  by  thresholding  residual  covariances 
above  3-a  of  the  sensor  model  uncertainty.  Each  algorithm  is  based  on  statistical 
assumptions  about  the  measurement  residual.  This  section  presents  the  statistical 
model  for  the  measurement  residual  and  two  algorithms  to  exploit  deviations  from 
this  model. 

Maybeck  [16]  presents  two  statistical  models  for  the  measurement  residual. 
First,  the  dehnition  of  the  measurement  residual  is 


r{ti)  =  z{ti)  -  H{ti)x{t^  )  (2.48) 

where  r{ti)  is  the  measurement  residual,  z{ti)  is  the  measurement  from  the  sensor, 
H{ti)  is  the  measurement  matrix,  and  )  is  the  hlter  state  estimate  just  before 
the  measurement  update  is  applied.  Equation  (2.49)  states  that  the  expected  value 
or  mean  of  the  measurement  residual  r{ti)  should  be  zero. 


E{r{U)}  =  0  (2.49) 

Equation  (2.50)  states  the  covariance  of  the  measurement  residual  r{ti)  should  match 
the  measurement  residual  covariance 
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E{r{U)r^{U)}  =  H{ti)P{t-)H^  {U)  + 


(2.50) 


where  the  measurement  residual  covariance  is  comprised  of  the  measurement  matrix 
the  hlter  covariance  before  the  measurement  update  is  P(t^  ),  and  the  sensor 
model  uncertainty  is  R{ti).  The  subsequent  paragraphs  present  two  applications  of 
the  statistical  measurement  residual  model. 

The  first  application  of  the  statistical  models  is  detection  of  a  sensor  failure.  A 
likelihood  function  with  a  moving  window  based  on  the  N  most  recent  measurement 
residuals  is  shown  in  Equation  (2.51)  [16] 


=  ct(«.)  -  1  Y.  <2-51) 

j=i-N+l  A 

where  Ck{ti)  is  a  slowly  varying  negative  value  uncorrelated  with  the  measurement 
residual,  N  determines  the  length  of  the  likelihood  function  window,  rl[tj)  is  the 
covariance  of  the  measurement  residual,  and  crKtj)  is  the  kth  diagonal  term  of  the 
residual  covariance  dehned  in  Equation  (2.50).  The  output  of  the  likelihood  func¬ 
tion  L^kiti)  is  compared  against  a  threshold  value.  The  likelihood  function  value 
becomes  more  negative  when  either  the  measurement  residual  incurs  a  bias  or  the 
covariance  increases  signihcantly.  A  significant  increase  in  covariance  is  characterized 
by  measurement  covariances  beyond  3-a  of  the  sensor  model  uncertainty. 

The  second  application  consist  of  applying  a  threshold  against  the  covariance 
statistic.  If  the  covariance  of  the  current  measurement  residual  exceeds  3-a  of  the 
sensor  model  uncertainty  it  is  rejected  and  no  measurement  update  occurs.  This 
method  is  used  in  this  thesis  to  help  reduce  error  introduced  by  outlier  radio  range 
measurements.  Equation  (2.52)  presents  the  logic  comparison 

f  Accept  r^(tj)  <  (Sa)^ 

Measurement  Decision  =  <  (2.52) 

I  Reject  r^(tj)  >  (Sa)^ 
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where  is  the  square  of  the  current  residual  and  is  the  rejection  threshold. 

The  variable  a  is  dehned  by  the  sensor  model  uncertainty.  Measurements  with  covari¬ 
ances  greater  than  3-a  are  generally  considered  unacceptable.  This  algorithm  provides 
rejection  of  measurements  with  very  high  uncertainties.  However,  if  the  Kalman  hlter 
diverges,  large  measurement  residuals  occur  and  measurements  that  should  be  used 
to  correct  the  divergence  will  be  rejected  based  on  the  hard  rejection  threshold.  The 
adaptive  Kalman  hlter  algorithms  presented  in  the  next  two  sections  contain  methods 
to  deal  with  high  measurement  residuals  without  completely  rejecting  sensor  measure¬ 
ments. 

2.9.2  Sage-Husa  Algorithm.  As  mentioned  in  the  previous  section,  radio 
range  measurements  contain  varying  uncertainty  dependent  on  time-synchronization 
and  the  RF  environment.  The  AKF  enables  estimation  of  the  vehicle  and  sensor  model 
uncertainty  during  hlter  operation.  As  the  uncertainty  in  the  radio  measurements 
increase,  the  AKF  increases  the  EKF  radio  model  uncertainty.  However,  the  AKF 
does  not  account  for  the  non-zero  mean  of  the  range  error,  which  dictates  the  need 
for  a  more  advanced  adaptive  algorithm. 

The  Sage-Husa  adaptive  algorithm  presents  an  algorithm  where  the  hrst  two 
moments  (mean  and  variance)  of  the  errors  may  be  estimated  for  both  the  vehicle 
dynamics  and  sensor  models  [17,22,28].  The  Sage-Husa  algorithm  performs  a  time- 
history  average  of  the  mean  and  variance  calculations  to  smooth  the  estimation.  Un¬ 
like  the  AKF,  Sage-Husa  requires  storage  of  only  the  last  mean  and  variance  estimate, 
signihcantly  reducing  computer  storage  requirements.  The  Sage-Husa  algorithm  op¬ 
erates  without  the  long  summation  operations  present  in  the  AKF,  further  reducing 
the  computation  resource  requirements.  This  section  continues  with  presentation  of 
the  Sage-Husa  adaptive  algorithm  and  then  the  implementation  equations  necessary 
for  inclusion  into  a  Kalman  hlter. 

2.9.2. 1  Estimation  Algorithm.  This  section  presents  the  Sage-Husa 
algorithm  equations  for  the  sensor  model  error  and  then  for  the  vehicle  dynamics 
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model  error.  For  the  sensor  model,  r{ti)  contains  the  estimated  mean  and  R{ti) 
contains  the  estimated  variance  of  the  sensor  model  error.  Eqnation  (2.53)  presents 
the  estimation  of  the  mean  of  the  sensor  model  error  [28] 


=  (1  -  d)f{ti)  +  d[z{ti+i)  -  (2.53) 

where  f{ti+i)  is  the  new  estimated  mean  of  the  sensor  model  error,  the  scalar  d  con¬ 
trols  the  weight  placed  on  the  new  mean  estimates,  r{ti)  is  the  previous  estimated 
mean  of  the  sensor  model  error,  z(tj_|_i)  is  the  sensor  measurement,  i?(tj+i)  is  the 
measurement  matrix,  and  is  the  current  hlter  state  estimate.  The  computa¬ 

tion  H (ti+i)x(t~^-^^)  presents  the  expected  measurement.  Subtraction  of  the  expected 
measurement  from  the  sensor  measurement  z{ti^i)  returns  the  measurement  resid¬ 
ual  or  error  in  the  expected  measurement.  The  average  of  the  measurement  residual 
provides  an  estimate  of  the  mean  of  the  sensor  model  error. 

Estimation  of  the  variance  or  second  moment  of  the  measurement  residual  for 
the  sensor  model  error  is  performed  in  two  steps.  First,  the  estimated  mean  is  removed 
from  the  measurement  residual  as  shown  in  Equation  (2.54) 


z{ti+i)  =  z{ti+i)  -  H{ti+i)x{ti^^)  -  (2.54) 

where  z(tj+i)  is  the  estimate  of  the  zero-mean  measurement  residual,  z{ti^i)  is  the 
sensor  measurement,  is  the  measurement  matrix,  x{t~_^i)  is  the  current  hlter 

state  estimate,  and  r(tj+i)  is  the  average  mean  of  the  sensor  model  error.  Next, 
Equation  (2.55)  presents  computation  of  the  variance,  or  uncertainty,  of  the  sensor 
model 


R{ti+i)  =  (1  -  d)R{ti)  +  d[z{ti+i)z{ti+if  -  H{ti+i)P{t^j^^)H{ti+i)'^]  (2.55) 
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where  R{ti^i)  is  the  new  estimated  variance  of  the  sensor  model  error,  the  scalar 
d  controls  the  weight  placed  on  the  new  mean  estimates,  R(ti)  is  the  previous  es¬ 
timated  variance  of  the  sensor  model  error,  z{ti+i)  is  the  estimate  of  the  zero- mean 
measurement  residual,  is  the  measurement  matrix,  and  is  the  current 

hlter  uncertainty.  The  computation  presents  the  uncertainty  of  the 

current  measurement  update.  The  computation  H(ti+i)P(t~^^)H(ti+i)'^  presents  the 
expected  variance  of  the  current  measurement  update.  Subtraction  of  the  actual  and 
expected  variance  presents  the  error  in  the  previous  uncertainty  R{ti)  of  the  measure¬ 
ment  model  error.  This  error  is  used  to  correct  the  sensor  model  uncertainty.  The 
Sage-Husa  algorithm  for  the  vehicle  dynamics  model  is  presented  next. 

For  the  vehicle  dynamics  model,  q{ti)  contains  the  estimated  mean  and  Q^iti) 
contains  the  estimated  variance  of  the  vehicle  dynamics  model  error.  Equation  (2.56) 
presents  the  estimation  of  the  mean  of  the  vehicle  dynamics  model 

qiU+i)  =  (1  -  d)q{ti)  +  d[x{tf_^^)  -  (2.56) 

where  g(fj+i)  is  the  new  estimated  mean  of  the  vehicle  dynamics  model  error,  the 
scalar  d  controls  the  weight  placed  on  the  new  mean  estimates,  q{ti)  is  the  previ¬ 
ous  estimated  mean  of  the  vehicle  dynamics  model  error,  x{t^_^-^^)  current  hlter  state 
estimate,  $(tj+i,fi)  is  the  state  transition  matrix,  and  x{tf)  is  the  previous  hlter 
state  estimate.  The  computation  x(tf_^^)  —  ^(ti+i,ti)x(tf)  presents  the  small  change 
or  perturbation  in  the  state  estimate.  The  time-history  average  performed  on  this 
perturbation  provides  an  estimate  of  the  mean  of  the  vehicle  dynamics  model  error. 
Next,  estimate  of  the  variance  of  the  vehicle  model  error  is  presented. 

Equation  (2.57)  shows  the  estimate  of  the  variance  of  the  vehicle  dynamics 
model  error 
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Qd{ti+i)  =  (1  -  d)Qd{ti)  +  d[K{ti+i)z{ti+i)z{ti+iY 


(2.57) 


where  (5d(^*+i)  is  the  new  estimated  variance  of  the  vehicle  dynamics  model  error,  the 
scalar  d  controls  the  weight  placed  on  the  new  mean  estimates,  Q^idi)  is  the  previous 
estimated  variance  of  the  vehicle  dynamics  model  error,  is  the  Kalman  gain, 

z{ti+i)  is  the  estimate  of  the  zero- mean  measurement  residual,  is  the  current 

hlter  uncertainty,  is  the  state  transition  matrix,  and  Pit^)  is  the  previous 

hlter  uncertainty. 

The  measurement  residual  variance  z{ti)z{ti)'^  is  pre  and  post-multiplied  by  the 
Kalman  gain  K{ti)  to  obtain  the  state  perturbation  uncertainty.  This  constitutes  the 
measured  variance  of  the  vehicle  dynamics  model  error.  The  computation  of 

Pitd)  -  (2.58) 

gives  the  expected  variance  of  the  vehicle  dynamics  model  error.  Subtraction  of  the 
measured  and  expected  variance  presents  the  error  in  the  previous  vehicle  dynamics 
model  uncertainty  Q^iti).  This  error  is  used  to  correct  the  estimated  variance  of  the 
vehicle  dynamics  model  error.  The  corrected  variance  estimate,  Qrf(tj+i),  is  used  as 
the  new  vehicle  dynamics  model  uncertainty.  The  next  section  covers  implementation 
of  Sage-Husa  algorithm  in  the  time  propagation  and  measurement  update  Kalman 
hlter  steps. 


2. 9. 2. 2  Kalman  Filter  Implementation.  The  previous  section  presents 
the  algorithms  to  estimate  the  mean  and  variance  of  the  vehicle  dynamics  and  sensor 
model  errors.  Application  of  these  estimates  within  the  Kalman  hlter  is  presented 
next.  The  two  equations  comprising  time  propagation  step  are  presented  hrst. 
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Equation  (2.59)  presents  correction  of  the  mean  error  in  the  state  vector  prop¬ 
agation  [28] 


+  qiti)  (2.59) 

where  is  the  new  hlter  state  estimate  corrected  with  the  average  mean  of  the 

vehicle  dynamics  model  error,  $(fj+i,tj)  is  the  state  transition  matrix,  x{t'l)  is  the 
current  state  estimate,  and  q{ti)  is  the  estimated  mean  of  the  vehicle  dynamics  model 
error.  The  addition  of  estimated  mean  q{ti)  as  opposed  to  subtraction  is  due  to  the 
subtraction 


-  ^{ti+i,ti)x{tt)  (2.60) 

performed  in  Equation  (2.56).  The  sign  convention  is  chosen  to  reduce  the  error  of 
the  mean  in  the  state  estimate. 

Equation  (2.61)  presents  the  corrected  hlter  uncertainty  propagation  [28] 

-P(tj+i)  =  ^{ti+i,ti)P{t'l)^{ti+i,tiY'  +  GdQdiii+i)G^  (2-61) 

where  is  the  new  hlter  uncertainty,  $(fj+i,fi)  is  the  state  transition  matrix, 

Pit'l)  is  the  current  hlter  uncertainty,  Gd  is  the  noise  matrix,  and  Qd(fi+i)  is  fhe 
estimated  vehicle  dynamics  model  uncertainty.  The  measurement  update  step  is  pre¬ 
sented  next. 

Three  equations  comprise  the  measurement  update  algorithm.  First,  the  Kalman 
gain  is  computed  in  Equation  (2.62) 


K(f,+i)  =  P(t- jE/^(f,+i)[T/(t,+i)P(t- jEf^(t,+i)  +  R{U+i)]-^  (2.62) 
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where  K{ti+i)  is  the  Kalman  gain,  is  the  current  hlter  uncertainty, 

is  the  measurement  matrix,  and  R{ti+i)  is  the  estimated  sensor  model  uncertainty. 

Second,  Equation  (2.63)  shows  the  update  of  the  hlter  states  [28] 


+  K{ti+i)[z{ti+i)  -  -  f{ti+i)]  (2.63) 

where  is  the  new  state  estimate,  x{t~j^^)  is  the  current  state  estimate,  K{tiJ^i) 

is  the  Kalman  gain,  is  the  sensor  measurement,  i?(tj+i)  is  the  measurement 

matrix,  and  f(tj+i)  is  the  estimated  mean  of  the  sensor  model  error.  The  subtraction 
of  the  estimated  mean  r(tj+i)  provides  the  zero- mean  measurement  residual,  presented 
previously  in  Equation  (2.54),  to  update  with  hlter  states. 

Last,  the  hlter  uncertainty  is  updated  as  shown  in  Equation  (2.64) 

=  ^’(V+i)  -  *:(«.+, )H(«.+,)P(«-+,)  (2.64) 

where  is  the  new  hlter  uncertainty,  P(t~^i)  is  the  current  hlter  uncertainty, 

is  the  Kalman  gain,  and  iT(ti+i)  is  the  measurement  matrix.  The  estimate 
of  the  sensor  model  uncertainty  is  used  in  Equation  (2.62)  to  compute  the  Kalman 
gain.  Through  the  Kalman  gain,  the  sensor  model  uncertainty  estimate  ehects  the 
hlter  uncertainty  update  shown  in  Equation  (2.64). 

The  topics  presented  in  this  background  chapter  prepare  the  reader  to  under¬ 
stand  the  specihc  application  of  coordinate  frames,  Kalman  hltering,  sensor  models, 
statistical  analysis,  and  adaptive  hltering  which  follow  in  the  subsequent  chapters. 
Chapter  III  presents  the  navigation  system  which  uses  an  EKE  and  associated  mod¬ 
els  to  estimate  navigation  trajectories.  Chapter  IV  contains  the  results  and  statistical 
analysis  from  several  data  collections  conducted  to  evaluate  the  performance  of  the 
navigation  system. 
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III.  Methodology 


The  purpose  of  the  radio  range  measurement  is  to  constrain  the  drift  present  in 
the  INS  when  GPS  is  not  available.  To  ascertain  the  trajectory-drift  constraint 
capability  of  the  DH500  radio  system,  a  sensor  platform  is  created  and  a  set  of  data 
collects  are  performed.  The  EKF  post-processing  implementation  and  sensor  platform 
are  detailed  in  Section  3.1.  Each  of  the  indoor  and  outdoor  data  collects  are  detailed 
in  Section  3.2. 

3. 1  Navigation  System 

The  navigation  system  consists  of  a  sensor  platform  and  post-processing  algo¬ 
rithms  used  to  compute  the  navigation  trajectory.  Section  3.1.1  covers  the  placement 
and  conhguration  for  each  sensor.  Section  3.1.2  presents  the  EKF  update  models 
for  each  sensor  along  with  the  specihc  post-processing  algorithms  used  to  obtain  the 
navigation  solution. 

3.1.1  Sensor  Platform.  The  sensor  platform  anchors  the  sensor  suite  used  in 
this  thesis  and  dehnes  a  position  and  orientation  for  each  sensor  relative  to  the  sensor 
platform’s  6-frame.  The  sensor  suite  used  in  this  thesis  consists  of  an  HG1700  tactical- 
grade  INS  [19],  DH500  radio  system,  a  stereo-camera  imaging  system  [26],  and  the 
SPAN  differential  GPS  (DGPS)  receiver  which  provides  centimeter-level  positioning 
accuracy. 

Figure  3.1  presents  the  physical  layout  of  the  sensor  used  in  this  thesis.  Two  of 
the  DH500  radios  are  set  at  DGPS-surveyed  locations.  These  stationary  radios  are 
labeled  base  station  1  (BSl)  and  base  station  2  (BS2).  The  third  radio,  labeled  mobile 
station  (MS),  is  attached  to  the  sensor  platform.  The  range  measurements  between 
each  BS-MS  radio  pair  are  labeled  ri  and  r2.  The  INS,  stereo-camera  imaging  system, 
and  DGPS  receiver  are  also  attached  to  the  sensor  platform.  A  barometer  is  simulated 
by  corrupting  truth  data  from  the  DGPS  for  the  outdoor  data  collections  and  from  the 
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Figure  3.1:  Overview  of  Navigation  System  Setup.  Note  the  two  stationary  DH500 
radios  BSl  and  BS2  provide  slant  range  measurements  ri  and  r2  for  constraint  of  the 
navigation  drift. 

height  of  the  sensor  platform  above  surveyed  markers  for  the  indoor  data  collections. 
The  simulated  barometer  data  is  used  to  constrain  vertical  navigation  axis  drift. 

Figure  3.2  illustrates  the  vectors  and  DCMs  which  dehne  each  sensor’s  position 
and  orientation  relative  to  the  sensor  platform  &-frame.  The  origin  of  the  6-frame  is 
dehned  to  coincide  with  the  origin  of  the  INS  s-frame.  Also,  the  orientation  of  the  INS 
is  rotated  to  align  with  the  6-frame.  These  INS  dehnitions  simplify  the  navigation 
trajectory  calculations.  The  position  and  orientation  of  the  GPS  antenna  is  dehned 
as  IbsQ  and  Cl^ ,  where  the  subscript  bsc  indicates  a  vector  in  the  6-frame  pointing 
to  the  GPS  antenna  s-frame.  Each  camera  has  a  separate  position  and  orientation. 
The  variables  hsa  and  C'l^^  contain  the  position  and  orientation  for  camera  1.  For 
camera  2,  lbsc2  and  contain  the  position  and  orientation.  The  position  and 

orientation  of  the  MS  radio  is  contained  in  hsMs  and 

The  data  from  each  sensor  are  stored  on  laptop  computers  also  located  on  the 
sensor  platform.  After  the  data  collections,  the  sensor  data  are  transfered  to  larger. 
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stereo  Vision  Body  Coordinate 

Camera  System  System 


Figure  3.2:  Position  &  Orientation  of  Mobile  Sensors  on  Vehicle.  A  location  vector 
lb  and  orientation  DCM  Cl  is  defined  for  each  sensor  relative  to  the  6-frame.  The 
INS  sensor  s-frame  origin  is  dehned  to  be  the  origin  of  the  6-frame  and  INS  s-frame 
orientation  is  dehned  to  align  with  the  6-frame. 
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more  powerful  computers  to  conduct  post-processing  through  the  EKF  and  associated 
sensor  models  which  are  covered  in  the  next  section. 

Before  each  data  collection,  the  sensor  platform  must  be  conhgured.  The  DH500 
radio  system  has  two  frequency  ranges:  400MHz  and  900MHz.  Observations  from 
initial  tests  of  the  DH500  radio  system  show  the  900MHz  band  provides  less  noisy 
slant  range  measurements.  This  observation  may  be  due  to  400MHz  interference  in 
the  local  area. 

The  camera  system  requires  the  focus  of  each  lens  to  be  set  for  the  focal  depth  of 
the  features  to  be  tracked  in  the  target  environment.  The  cameras  are  focused  closer 
for  the  indoor  environment  and  farther  away  for  the  outdoor  environment.  A  camera 
calibration  must  be  performed  when  the  focus  setting  on  the  cameras  is  changed  to 
capture  the  shift  in  non-linear  lens  distortions. 

The  tactical-grade  HG1700  INS  is  connected  to  the  SPAN  DGPS  receiver  to  aid 
the  GPS  position  solntion  [19].  The  DGPS  system  reqnires  the  INS  be  aligned  prior 
to  sensor  platform  movement  [18].  The  INS-aided  DGPS  position  solntion  from  the 
SPAN  provides  position,  velocity,  and  attitnde  data.  This  data  is  used  as  a  “truth” 
data  source  for  the  outdoor  data  collection  to  compare  against  the  EKF  post-process 
trajectory. 

3.1.2  EKF  Implementation.  The  data  collected  from  the  sensor  platform 
are  post-processed  throngh  an  EKF  to  obtain  the  estimation  of  the  navigation  system 
trajectory.  The  EKF  implementation  used  in  this  thesis  extends  the  vision-aided 
navigation  (VAN)  system  developed  by  Veth  [26].  The  VAN  system  consists  of  tight 
integration  of  INS  and  stereo  camera  system  data.  The  details  and  models  used  to 
implement  the  VAN  system  are  contained  in  [26] .  The  EKF  baro-altitnde  model  used 
to  constrain  the  vertical  navigation  channel  drift  and  EKF  radio  range  model  used 
to  constrain  the  drift  in  the  horizontal  navigation  channels  are  presented  in  later 
snbsections. 

Eqnation  (3.1)  presents  the  states  estimated  by  the  EKF 
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where  6p"'  and  6v^  contain  the  error  in  position  and  velocity,  V’  contains  the  attitude 
of  the  sensor  platform,  and  contain  the  bias  estimates  for  the  accelerometers 
and  gyroscopes,  and  h  contains  the  estimate  of  the  baro-altitude  aiding  bias.  The 
super-script  n  indicates  the  n-frame  is  used  as  the  frame  of  reference. 

3. 1.2.1  Baro- Altitude  Model.  The  barometer  provides  measurements 
of  air  pressure  which  correlate  to  the  altitude  of  the  barometer  as  presented  in  Sec¬ 
tion  2.7.  The  baro-altitude  EKF  measurement  update  model  is  used  to  constrain  the 
INS  drift  present  in  the  vertical  navigation  channel.  Vertical  channel  information  is 
pulled  from  the  DGPS  truth  data  for  the  outdoor  data  collections.  For  the  indoor 
data  collections  the  height  of  the  sensor  platform  above  the  surveyed  indoor  markers 
is  used  for  the  vertical  channel  information.  The  next  paragraphs  present  the  FKF 
baro-altitude  model. 

The  model  presented  assumes  the  baro-altitude  aiding  measures  vertical  axis 
position  of  the  sensor  platform  in  the  n-frame.  The  baro-altitude  measurements  are 
modeled  as  the  true  vertical  axis  position  plus  white,  Gaussian  noise  and  a  time- 
varying  bias.  Equation  (3.2)  presents  the  model 
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PdW  =  Pdit)  +  Wsit)  +  hit) 


(3.2) 


where  p^it)  is  the  true  vertical  position,  WBif)  is  the  white  noise  of  strength  h{t) 
is  the  barometer  bias  estimated  by  the  EKF,  and  is  the  corrupted  vertical  axis 
measurement.  For  the  true  vertical  position  the  superscript  n  indicates  the 

n-frame  and  the  subscript  d  indicates  the  down-axis  of  the  NFD  n-frame. 

The  vertical  axis  measurement  dp^{t)  correlates  directly  to  the  p^{t)  system 
state  estimate  error.  Equation  (3.3)  presents  the  measurement  matrix  in  terms  of  the 
EKF  system  states. 


H{ti)  =  [  0  0  1  0  •••  0  1  ]  (3.3) 

The  hnal  EKF  measurement  model  for  baro-altitude  aiding  is  presented  in  Equa¬ 
tion  (3.4) 


5z{ti)  =  [{)  0  1  0  •••  0  l]5x{t^) +Vb{ti)  (3.4) 

where  z{ti)  is  the  estimated  measurement,  x{t~)  is  the  current  EKF  state  estimate, 
and  Vhiti)  is  the  measurement  noise  strength  dehned  by 

As  mentioned  in  Section  3.1.1,  the  EKF  baro-altitude  aiding  model  is  not  used 
to  include  real  barometer  information,  but  to  incorporate  slightly  corrupted  truth 
data  to  constrain  the  drift  of  the  vertical  navigation  axis  to  sub-meter  levels.  The 
uncertainty  of  the  EKF  baro-altitude  model  is  set  to  0.1  meters.  The  vertical  channel 
truth  data  from  the  DGPS  for  the  outdoor  collects  and  surveyed  floor  markers  for  the 
indoor  collects  are  corrupted  with  white,  Gaussian  noise  of  strength  at  =  0.1. 

3. 1.2. 2  DH500  Radio  Model.  The  slant  range  measurements  provided 
by  the  DH500  radio  system  contains  information  about  the  position  of  the  sensor 
platform.  This  correlates  to  the  position  states  of  the  EKF  state  vector.  The  range 
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error  is  modeled  as  the  true  range  corrupted  with  white  Gaussian  noise.  Equation  (3.5) 
presents  the  model 


r(t)  =  r(t)  +  VR{t)  (3.5) 

where  the  slant  range  provided  by  the  DH500  is  r{t),  the  true  slant  range  is  r(t), 
and  the  white  Gaussian  noise  source  describing  the  uncertainty  of  the  measurement  is 
vii{t)  with  strength  The  value  chosen  for  is  determined  from  the  RMS  range 
error  collected  from  the  stationary  data  collections. 

The  hrst  step  is  to  model  the  slant  range  measurement  in  terms  of  the  EKF 
position  states.  Equation  (3.6)  shows  the  computation  of  the  true  slant  ranges 

P"ii)+lbsMS-PBS2\  _ 

where  p'^{t)  is  the  position  of  the  sensor  platform  ,  I^sms  position  of  the  MS 

radio  on  the  sensor  platform,  and  p^gi  and  p%s2  positions  of  the  ground 

radios  BSl  and  BS2.  These  truth  computations  are  performed  in  the  n-frame. 

The  uncertainty  of  each  of  the  slant  range  measurements  is  dehned  by  two 
independent,  white  Gaussian  noise  sources  v^it).  Equation  (3.7)  presents  the  vector 
of  noise  sources 


r{t)  = 


n{t) 

G(t) 


VR{t) 


Vi{t) 

V2it) 


(3.7) 


where  the  strength  of  each  noise  source  is  dehned  by  and  a\2-  These  noise  sources 
are  set  to  the  same  uncertainty  for  the  EKF  radio  range  model. 

The  measurement  model  must  be  linearized  for  implementation  in  the  EKF. 
Equation  (3.8)  presents  the  matrix  of  partial  derivatives 
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H[x{t^  ),ti 


(3.8) 


dri 

dri 

dr\ 

dp^ 

dp" 

dp" 

dr2 

dr2 

dr2 

.  dp" 

dp" 

dPd 

where  is  the  partial  derivative  of  the  slant  range  eqnation  between  the  MS  and 

^Pn 

BSl  with  respect  to  the  respect  to  the  north  (n)  axis  in  the  n-frame.  The  results  of 
the  partial  derivatives  are  shown  in  Equation  (3.9)  with  the  denominators  ki  and  /c2 
shown  in  Equation  (3.10). 


H[x{t^  ),ti 


(p"(t)  +  -  PBSif  /ki  0  •••  0 

(p"(t)  +  /k2  0  •••  0 


h  =  +  lbs -  p BSl 

k2  =  Ip'^it)  +  hsn  -  PbS2 


Equation  (3.11)  presents  the  linearized  measurement  model 


(3.9) 


(3.10) 


z{ti)  =  H[x{ti  ),ti\x{ti  )  +  v{ti) 


(3.11) 


where  the  measurement  matrix  H[x{t~),ti]  is  re-evaluated  about  the  current  state  es¬ 
timate  x{t~)  before  each  slant  range  measurement  update.  This  allows  non-linearities 
in  the  slant  range  model  to  be  incorporated  into  the  EKE  measurement  update.  The 
EKE  model  uncertainty  for  each  of  the  radio  pairs  is  contained  in  the  measurement 
noise  matrix  R{ti)  shown  in  Equation  (3.12). 


R(ti) 


(^Rl  0 
0 


(3.12) 


The  EKE  radio  range  model  presented  assumes  the  error  of  the  slant  ranges 
is  zero-mean  and  white  Gaussian  in  distribution.  Analysis  presented  in  Chapter  IV 
reveals  the  range  error  has  a  notable  positive  bias  and  the  distribution  becomes  in- 
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creasingly  non-Gaussian  as  RF  interference  increases.  This  violates  both  EKF  radio 
range  model  assumptions.  To  account  for  the  variation  in  uncertainty  of  the  radio 
ranges,  the  residual  monitoring  technique  presented  in  Section  2.9.1  is  applied  to  the 
EKF  radio  range  model.  A  slight  improvement  is  noted  in  the  post-processing  trajec¬ 
tory  results,  but  residual  monitoring  does  not  account  for  the  non-zero  mean  of  the 
radio  range  error. 

The  residual  monitoring  method  is  swapped  for  a  modihed  implementation  of 
the  Sage-Husa  adaptive  algorithm.  Only  the  measurement-update  portion  of  the  Sage 
Husa  algorithm  is  implemented  in  the  radio  range  model  due  to  time  constraints.  As 
presented  in  Section  2.9.2,  the  Sage  Husa  algorithm  estimates  the  mean  of  the  mea¬ 
surement  residual  and  adjusts  the  measurement  uncertainty  R{ti)  as  the  error  in  the 
range  measurements  change.  The  Sage-Husa  adaptive  algorithm  and  its  implemen¬ 
tation  are  covered  in  Section  2.9.2.  The  Sage-Husa  algorithm  must  be  tuned  via  its 
memory  factor  d. 

3.2  Data  Collections 

Each  data  collection  consists  of  specihc  sensor  placement  and  trajectory  deh- 
nition.  For  stationary  data  collections,  the  trajectory  consists  of  a  single  surveyed 
location.  For  moving  data  collections,  the  trajectory  consists  of  a  predetermined  path 
which  the  sensor  platform  follows.  The  next  sections  present  the  data  collection  se¬ 
tups  in  the  following  order:  stationary  outdoor  collection,  moving  outdoor  collection, 
stationary  indoor  collections,  and  moving  indoor  collections.  The  stationary  data 
collections  proceed  the  moving  data  collections  to  incorporate  changes  in  the  EKF 
radio  range  model  based  on  the  effects  of  the  outdoor  and  indoor  environments  on 
the  DH500  radio  system  performance. 

3.2.1  Stationary  Outdoor  Collect.  An  outdoor  environment  with  minimal 
RF  interference  is  chosen  to  perform  the  stationary  outdoor  data  collect.  Free  from 
obstructions,  the  DH500  radio  system  has  clear  LOS  between  each  radio.  This  envi- 
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Figure  3.3:  Outdoor  Stationary  Data  Collection  Setup.  The  displayed  locations  of 
the  base  stations  and  mobile  station  are  chosen  to  provide  the  clearest  LOS  and  low 
RF  interference  between  the  radios. 

ronment  allows  characterization  of  the  lowest  expected  radio  range  error,  providing 
the  best  expected  INS  drift  constraint  capability.  Figure  3.3  displays  the  approximate 
position  of  each  of  the  three  DH500  radios. 

The  geometry  between  the  radio  positions  models  a  real-world  usage  applica¬ 
tion.  More  ideal  radio  placement  is  described  by  an  equilateral  triangle.  Real-world 
situations  may  not  always  allow  for  equilateral  radio  placement.  The  radio  placement 
produces  high  position  uncertainty  in  the  east-to-west  axis.  The  north-to-south  axis 
has  much  lower  uncertainty  due  to  the  wide  radio  placement. 

Once  the  radios  are  positioned  as  approximately  shown  in  Figure  3.3,  each  radio 
position  is  surveyed  with  the  SPAN  DGPS  receiver.  A  10-minute  data  collection  is 
performed  with  the  laptop  computer  connected  to  the  radio  system  providing  over  200 
data  points  for  each  radio  pair  to  characterize  the  range  error  of  the  radios.  During 
post-processing,  the  true  slant  range  between  each  radio  pair  is  computed  using  the 
DGPS  survey  data.  The  slant  range  measurements  collected  from  the  DH500  radio 
system  are  subtracted  from  the  true  slant  ranges  to  obtain  the  slant  range  error.  The 
mean  and  variance  are  computed  to  determine  the  RMS  error  of  the  slant  ranges. 
The  RMS  error  is  used  to  dehne  the  uncertainty  for  the  EKF  radio  range  model.  To 
determine  whether  the  distribution  of  the  range  error  is  Gaussian  the  Ghi-Squared 
statistical  test  is  performed  and  a  multi-bin  histogram  is  generated.  Graphs  and 
analysis  for  the  outdoor  stationary  data  collection  are  presented  in  Section  4.1.1. 
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(a)  North-South  “a” 


(b)  North-South  “b” 


Figure  3.4:  Outdoor  Moving  Data  Collection  Setup.  The  locations  of  the  base 

stations  do  not  change  from  the  stationary  data  collection  to  the  moving  collection. 
The  sensor  platform  is  moved  along  the  track  indicated  by  the  arrows. 


3.2.2  Moving  Outdoor  Collect.  To  test  the  INS  drift  constraint  capability  of 
the  DH500  radio  system,  an  outdoor  moving  data  collection  is  performed.  The  INS, 
DH500  radio  system,  stereo-camera  image  system,  and  GPS  comprise  the  sensors 
used  for  the  outdoor  moving  collect.  Prior  to  the  outdoor  moving  data  collections, 
the  SPAN  The  lenses  on  the  stereo  camera  system  are  set  to  focus  on  distant  objects. 
Several  laptop  compnters  are  used  to  collect  data  from  each  of  the  sensors.  Figure  3.4 
displays  the  radio  placement  and  pre-determined  trajectory  of  the  sensor  platform. 

Similar  to  the  outdoor  stationary  data  collection,  two  of  the  DH500  radios  are 
positioned  at  stationary,  surveyed  locations  marked  BSl  and  BS2.  A  golf  cart  is  nsed 
to  transport  the  sensor  platform  along  the  trajectory  shown  in  Figure  3.4  indicated  by 
the  white  arrows.  A  total  of  fonr  data  collections  are  performed.  Two  data  collections 
follow  the  trajectory  shown  in  Figure  4(a)  are  designated  with  “a.”  The  other  two 
data  collections  follow  the  trajectory  shown  in  Fignre  4(b)  are  designated  with  “b.” 
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An  incremental  approach  is  taken  for  post-processing  the  data.  EKF  post¬ 
processing  is  conducted  first  with  only  INS  data.  Next,  EKF  bias  estimation  is  added. 
To  allow  the  EKF  to  estimate  the  bias  states,  a  2-minute  hlter  alignment  is  performed 
with  truth  data  from  the  SPAN.  Next,  the  radio  ranges  and  corrupted  DGPS  vertical 
axis  data  is  added  to  constrain  the  drift  present  in  the  INS  with  bias  estimation 
trajectory.  The  residual  monitoring  method  and  Sage-Husa  adaptive  method  are 
tested  separately  in  the  EKF  radio  range  model.  This  incremental  approach  ensures 
each  data  source  is  being  post-processed  correctly  before  additional  data  is  added.  The 
stereo  image  data  was  omitted  from  post-processing  due  to  a  poor  camera  calibration 
which  was  unable  to  be  corrected  due  to  time  constraints.  Without  a  good  stereo 
camera  calibration,  the  EKF  is  not  able  to  properly  use  the  image  data  due  to  the 
lens  distortion.  Section  4.2  presents  the  results  and  analysis  for  the  moving  outdoor 
data  collection. 

The  sensor  platform  was  then  placed  indoors  where  RE  interference  will  intro¬ 
duce  signihcant  error  into  the  radio  range  measurements.  The  post-processing  results 
from  the  indoor  data  collections  will  determine  how  well  the  DH500  radio  system  is 
able  to  constrain  the  INS  drift  while  indoors. 

3.2.3  Stationary  Indoor  Collects.  The  indoor  data  collection  setup  consists 
of  two  DH500  radios  placed  outdoors.  The  sensor  platform  consists  of  the  INS,  MS 
radio,  and  stereo  camera  system  is  positioned  indoors.  Only  the  radios  positioned 
outside  have  clear  LOS  view  of  each  other.  The  RE  signals  must  now  travel  through 
the  walls  of  the  building  to  reach  the  MS  radio  located  indoors.  The  building  walls 
reflect  the  RE  signals,  generating  interference  which  results  in  a  predominantly  posi¬ 
tive  bias  in  the  range  error.  The  EKF  radio  model  uncertainty  must  be  re-estimated 
to  account  for  the  additional  errors  now  present  in  the  radio  range  measurements  due 
to  increased  RE  interference.  Figure  3.5  illustrates  the  position  of  the  two  DH500 
radios  placed  outside.  The  positions  of  the  outdoor  BSl  and  BS2  radios  are  surveyed 
with  DGPS. 
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Figure  3.5:  Outdoor  Radio  Positions  for  Indoor  Data  Collections.  The  position  of 
the  outside  radios  simulate  potential  locations  of  radio  base  stations  used  in  the  field. 
See  Figure  3.6  for  indoor  hallway  layout. 

Similar  to  the  outdoor  data  collections,  the  radio  placement  is  indicative  of  a 
real-world  application  as  opposed  to  the  more  ideal  equilateral  placement.  This  radio 
placement  introduces  additional  non-idealities  into  the  performance  results  which  are 
presented  in  Section  4.1.2  and  4.3. 

Figure  3.6  displays  the  surveyed  indoor  locations  where  the  third  DH500  radio  is 
placed.  The  static  indoor  data  collections  consist  of  extracting  the  stationary  portions 
of  the  indoor  moving  trajectories  presented  in  the  next  section.  The  stationary  data 
sets  for  the  north  and  south  surveyed  point  are  analyzed  in  separate  groups.  The 
accumulated  stationary  collection  time  for  each  radio  pair  at  the  north  and  south 
surveyed  points  is  abont  eight  minntes.  This  provides  over  100  data  points  for  each 
radio  pair.  The  true  slant  ranges  are  computed  from  the  surveyed  outdoor  radio 
locations  and  surveyed  indoor  points.  The  true  slant  range  is  subtracted  from  the 
measured  radio  ranges  obtained  from  the  DH500  radio  system  to  compnte  the  range 
error.  The  RMS  error  calculation  from  the  mean  and  variance  of  the  range  error  is  used 
to  determine  a  new  nncertainty  for  the  EKF  radio  range  model.  Similar  to  the  ontdoor 
stationary  data  collection,  a  binned  histogram  is  generated  and  the  Chi  Sqnared  test 
is  performed  to  determine  the  fit  of  the  range  error  to  the  Gaussian  distribution.  The 
results  from  the  indoor  stationary  data  collection  found  in  Section  4.1.2. 
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Figure  3.6:  Indoor  Hallway  Surveyed  Locations  for  Stationary  Data  Collections. 

Locations  are  displayed  for  north  and  south  stationary  data  collections.  Note  the 
layers  of  building  walls  illustrated  by  the  building  layout.  These  walls  attenuate  the 
radio  RF  signals  and  reflect  the  RF  signals  causing  RF  interference. 
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(a)  North-South  “a”  (b)  North-South  “b” 


Figure  3.7:  North-South  Indoor  Moving  Trajectory  Illustration.  The  surveyed 

points  provide  position  “truth”  data  to  compare  against  the  EKF  post-process  in¬ 
door  trajectory.  Note  changes  in  trajectory  direction  between  Figure  3.7  (a)  and 
3.7  (b)  indicated  by  the  black  arrow. 

3.2.4  Moving  Indoor  Collects.  The  results  from  the  stationary  indoor  data 
collection  show  a  signihcant  increase  in  radio  range  error  as  will  be  presented  in 
Section  4.3.  This  increase  in  error  reduces  the  ability  of  the  DH500  radio  system  to 
constrain  the  trajectory  drift  of  the  INS.  Two  groups  of  indoor  data  collections  are 
performed  to  evaluate  the  drift  constraint  capability  of  the  radio  system  operating 
with  signihcant  RF  interference. 

The  hrst  group  is  labeled  “North-South.”  Figure  3.7  presents  an  overview  of 
for  the  indoor  trajectories  contained  in  the  “North-South”  group.  A  total  of  four 
separate  data  collections  are  performed  for  the  “North-South”  group.  Data  collects 
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which  follow  the  trajectory  direction  in  Figure  3.7  (a)  are  designated  with  “a”  and 
trajectories  that  follow  Figure  3.7  (b)  are  designated  with  “b.” 

Two  of  the  DH500  radios  are  placed  outside  as  shown  previously  in  Figure  3.5. 
The  indoor  sensor  platform  consists  of  the  INS,  the  MS  DH500  radio,  and  the  stereo 
camera  imaging  system.  The  camera  system  focus  is  re-adjusted  for  the  indoor  data 
collections  to  provide  clear  focus  on  objects  near  the  cameras.  The  indoor  trajectory  of 
the  sensor  platform  is  illustrated  in  Figure  3.7.  The  performance  of  the  sensor  platform 
in  this  hallway  represents  the  best  expected  indoor  drift-constraint  performance  of  the 
radio  system  due  to  the  relatively  close  proximity  to  the  outdoor  radios. 

The  second  indoor  data  collection  consists  of  moving  the  sensor  platform  through 
the  building  in  a  square-shaped  trajectory.  This  data  collection  is  labeled  “Square.” 
A  variety  of  walls  and  doors  are  brought  between  the  MS  indoor  radio  and  the  outdoor 
BS  radios.  These  objects  further  increase  the  RF  interference.  The  outdoor  radios 
BSl  and  BS2  remain  in  the  same  locations  as  defined  in  the  North-South  data  col¬ 
lection.  The  North-South  data  collection  camera  focus  settings  are  re-used,  negating 
the  need  for  an  additional  camera  calibration.  Figure  3.8  illustrates  the  square  indoor 
moving  trajectory.  The  sensor  platform  starts  in  the  Advanced  Navigation  Technol¬ 
ogy  (ANT)  Lab  shown  in  the  upper-right  hand  corner  of  Figure  3.8  and  proceeds 
clockwise  around  the  square-shaped  trajectory.  The  sensor  platform  ends  back  at  the 
start  location  in  the  ANT  Lab. 

EKF  post-processing  of  both  the  North-South  and  Square  indoor  moving  data 
collections  proceeds  with  an  incremental  approach.  First,  only  the  INS  data  is  post- 
processed.  Then  EKF  bias  estimation  is  enabled.  Again,  a  2-minute  stationary  align¬ 
ment  is  performed  to  allow  the  filter  to  estimate  the  bias  states.  The  alignment  data  is 
obtained  from  the  SPAN  attitude  estimate  and  position  is  obtained  from  the  starting 
surveyed  floor  marker.  Next,  the  radio  range  and  corrupted  vertical  axis  truth  data 
is  incorporated.  A  proper  camera  calibration  is  obtained  for  the  indoor  lens  focus 
settings.  The  stereo  image  data  is  incorporated  last.  Analysis  of  each  step  of  the  in- 
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Figure  3.8:  Square  Indoor  Moving  Trajectory  Illustration.  The  square  indoor  mov¬ 
ing  trajectory  provides  variation  in  RF  interference  and  signal  attenuation. 
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cremental  build-up  ensures  each  data  set  is  incorporated  correctly  and  the  associated 
hlter  parameters  are  tuned  correctly.  The  Sage-Husa  adaptive  algorithm  is  used  in 
the  EKF  radio  range  model  for  all  the  indoor  data  collections.  Results  and  analysis 
for  the  indoor  moving  data  collections  are  presented  in  Section  4.3. 
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IV.  Results 


This  chapter  presents  results  and  analysis  for  the  data  collections  outlined  in 
Section  3.2.  Results  from  the  stationary  data  collected  for  both  the  outdoor 
and  indoor  environments  are  presented  in  Section  4.1.  Then,  the  results  for  the 
outdoor  moving  data  collections  are  contained  in  Section  4.2.  Section  4.3  contains 
results  for  the  indoor  moving  data  collections. 

4.1  Radio  Performance  Characterization 

This  section  presents  results  and  analysis  for  the  observed  range  errors  of  the 
DH500  radio  system.  The  purpose  of  characterization  is  to  measure  the  uncertainty 
of  the  range  error.  The  RMS  value  of  the  range  error  is  used  as  the  first  value  for  the 
radio  range  model  uncertainty.  This  model  uncertainty  is  then  further  tuned  during 
residual  analysis.  Residual  analysis  will  be  presented  in  Sections  4.2  and  4.3  for  each 
data  collection  environment. 

The  performance  of  the  radio  system  is  characterized  by  generating  histograms 
of  the  range  error.  The  histograms  approximate  the  statistical  distribution  of  the 
range  error.  Overlaid  on  top  of  the  histogram  is  the  plot  of  a  Gaussian  distrobution 
with  the  same  mean  and  variance  as  the  set  of  range  errors.  The  Chi-Squared  statis¬ 
tical  test  is  applied  to  each  set  of  range  errors  for  the  final  determination  whether  the 
set  of  range  errors  is  Gaussian  in  distribution. 

The  range  measurements  between  the  DH500  radios  are  referenced  in  two  pairs. 
Radio  pair  1  consists  of  BSl  and  the  MS.  Radio  pair  2  consists  of  BS2  and  the  MS. 
Section  4.1.1  presents  the  outdoor  range  characterization.  Section  4.1.2  presents  the 
indoor  range  characterization. 

4.1.1  Outdoor  Characterization.  The  first  procedure  characterizes  the  out¬ 
door  stationary  performance  of  the  DH500  radios.  The  navigation  system  is  setup  as 
described  in  Section  3.2.1.  Once  the  position  of  each  radio  is  surveyed,  a  10  minute 
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Radio  Range  Error  Over  Time  (Outdoor  Static  Collect) 


Figure  4.1:  Outdoor  Range  Error  Plot.  Spread  of  range  errors  is  due  to  noise  in 

the  range  measurement  system. 


data  collection  is  performed  to  gather  range  measurements  between  each  radio  pair. 
The  10  minute  data  collection  provides  over  200  range  measurements  per  radio  pair. 

The  raw  range  errors  used  in  the  outdoor  characterization  are  presented  in 
Figure  4.1.  The  majority  of  range  errors  are  distributed  evenly  with  a  mean  of  6.56 
meters.  The  spread  of  the  range  errors  is  due  to  noise  present  in  the  ranging  system. 

Histograms  representing  the  distribution  of  radio  pairs  1  and  2  are  shown  in 
Figures  4.2  and  4.3  respectively.  Tables  4.1  and  4.2  present  the  statistics  and  Chi 
Squared  GOF  results  for  each  set  of  range  error  distributions.  Analysis  of  radio  pair 
1  follows. 

Observe  the  histogram  and  Gaussian  overlay  shown  in  Figure  4.2.  Note  the  long¬ 
tailed  distribution  of  the  range  errors  which  indicate  a  non-Gaussian  distribution. 
The  observation  is  conhrmed  by  the  Ghi  Squared  GOF  test  returning  a  result  of 
non-Gaussian  presented  in  Table  4.1.  Although  only  two  ranges  fall  above  25  meter 
error,  the  case  of  extreme  outliers  must  be  handled  to  reduce  corruption  of  the  EKF 
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Histogram  and  Gaussian  Approximation  of  Range  Error  (Radio  Pair  1) 

60  - ^ - 1 - ^ ^ ^ - 


Range  Error  [m] 


Figure  4.2:  Outdoor  Range  Error  Between  Radio  Pair  1.  Note  how  outliers  at  30 
and  65  meters  create  a  long-tailed  distribution. 

estimated  trajectory.  The  mean  of  the  range  error  data  in  Table  4.1  presents  a  bias  in 
the  range  measurements.  Estimation  of  the  range  bias  within  the  radio  range  model 
is  a  possible  solution  to  reduce  the  effects  of  the  bias. 

From  further  experience  with  the  radios  it  is  found  that  the  bias  is  not  constant 
due  to  changes  in  RF  interference.  The  effects  of  RF  interference  are  present  in  the 
outlier  range  errors  shown  in  Figure  4.2.  Next,  the  distribution  of  radio  pair  2  is 
presented. 


Table  4.1:  Statistics  for  Radio  Pair  1  Range  Error.  The  range  distribution  has  a 
non-zero  mean  and  does  not  conform  to  the  Gaussian  distribution. 


Parameter 

Value 

Mean 

6.56  m 

Std  Dev 

7.60  m 

RMS  Error 

10.04  m 

Chi  Squared  Test 

Non-Gaussian 
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Histogram  and  Gaussian  Approximation  of  Range  Error  (Radio  Pair  2) 


Range  Error  [m] 

Figure  4.3:  Outdoor  Range  Error  Between  Radio  Pair  2.  Note  how  range  error 

distribution  appears  to  reasonably  fit  Gaussian  overlay. 

Figure  4.3  and  Table  4.2  present  the  range  error  distribution  and  statistics 
for  radio  pair  2.  First,  note  the  range  error  distribution  appears  to  £t  the  overlaid 
Gaussian  better  than  radio  pair  1.  Gonhrmation  of  the  observation  is  provided  by 
the  result  of  Gaussian  from  the  Ghi  Squared  GOF  test.  Note  also  the  mean  of  the 
distribution  is  much  closer  to  zero  than  radio  pair  1.  The  fairly  symmetric  distribution 
of  range  errors  around  zero  meters  presents  insight  into  the  range  error  introduced  by 
the  radio  system  itself.  RF  interference  from  multipath  can  add  positive  range  error. 
Since  the  range  calculation  algorithms  within  the  DH500  radio  system  are  proprietary, 
the  exact  source  of  the  outlier  positive  range  errors  is  not  known.  However,  as  will  be 
shown  for  the  indoor  range  error  analysis  in  the  next  section,  excessive  RF  interference 
produces  predominantly  positive  range  errors  for  the  DH500  radio  system.  The  lack 
of  excessive  positive  range  error  indicates  a  general  lack  of  RF  interference  for  radio 
pair  2.  Figure  4.3  displays  the  Gaussian-like  histogram  with  a  mean  of  zero  and  an 
uncertainty  of  6.5  meters. 
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Table  4.2:  Statistics  for  Radio  Pair  2  Range  Error.  The  range  distribution  is 

approximately  zero-mean  and  conforms  to  the  Gaussian  distribution. 


Parameter 

Value 

Mean 

0.04  m 

Std  Dev 

6.46  m 

RMS  Error 

6.46  m 

Ghi  Squared  Test 

Gaussian 

For  the  purposes  of  this  thesis,  the  low  RF  interference  outdoor  range  charac¬ 
terization  presents  a  lower  bound  on  range  errors  present  in  the  DH500  radio  system. 
Further  analysis  of  radio  range  errors  is  conducted  in  the  next  section  where  results 
from  the  stationary  indoor  range  error  characterization  are  presented 

4-1.2  Indoor  Characterization.  The  DH500  radio  system  is  used  to  aid 
indoor  navigation.  This  warrants  a  separate  characterization  of  the  range  error  per¬ 
formance.  High  RF  interference  introduced  by  the  indoor  environment  signihcantly 
changes  the  characteristics  of  the  radio  range  error. 

The  graphs  that  follow  show  the  severe  degradation  in  range  measurement  per¬ 
formance  as  the  walls  and  doors  of  the  indoor  environment  introduce  significant  RF 
interference  between  the  radios.  The  navigation  system  setup  for  the  stationary  in¬ 
door  data  collections  is  described  in  Section  3.2.3. 

The  presentation  format  of  the  indoor  characterization  data  follows  that  of 
the  outdoor  characterization.  For  the  indoor  analysis,  static  data  collections  are 
performed  at  two  locations  within  the  building.  These  are  differentiated  by  the  labels 
north  and  south  in  reference  to  locations  in  the  hallway.  First,  graphs  of  the  raw 
range  errors  between  radio  pair  1  and  2  are  presented.  Then  histograms  of  the  range 
error  data  with  overlaid  Gaussian  approximations  are  presented  and  analyzed.  Tables 
with  statistics  and  results  of  the  Ghi  Squared  GOF  test  for  the  range  error  data  follow 
each  histogram.  Gonclusions  from  the  indoor  range  error  analysis  are  presented  at 
the  end  of  this  section. 
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Figure  4.4:  Indoor  North  Range  Error  Plot.  A  large  number  of  extreme  outliers 
exist  above  200  meters  due  to  RF  channel  interference  from  the  building. 

Figures  4.4  and  4.5  present  the  raw  range  error  measurements  for  the  north  and 
south  stationary  data  sets,  respectively.  The  notable  difference  when  compared  with 
the  outdoor  data  in  Figure  4.1  is  the  large  range  errors  presented  by  the  outliers.  This 
is  a  prominent  display  of  performance  degradation  due  RF  interference. 

Figures  4.6  and  4.7  present  long-tailed  histograms  for  both  radio  pairs  at  the 
north  position.  The  indoor  radio  MS  is  located  closer  to  BSl  than  BS2.  The  surveyed 
distance  between  MS  and  BSl  is  76.6  meters  and  between  MS  and  BS2  is  112.0 
meters.  The  RF  channel  between  radio  pair  1  is  shorter  and,  from  observation  during 
the  data  collection,  the  building  presents  less  interference  to  the  RF  channel  between 
these  radios.  The  opposite  is  true  for  radio  pair  2.  Almost  the  entire  width  of  the 
building  stands  between  radio  pair  2  which  provides  abundant  RF  interference.  This 
RF  channel  difference  is  apparent  in  Figures  4.6  and  4.7  by  noting  the  length  of  the 
range  error  histogram  tails.  The  more  RF  channel  interference  present,  the  longer 
the  histogram  tail.  It  is  interesting  to  note  the  strong  presence  of  range  error  around 


Radio  Range  Error  Over  Time  (North  End  of  Hallway) 
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Figure  4.5:  Indoor  South  Range  Error  Plot.  The  ranges  are  reasonable  contained 
below  100  meters  as  compared  with  the  extreme  outliers  shown  in  Figure  4.4. 

225  meters  for  radio  pair  2.  This  suggests  strong  RF  interference  along  a  trajectory 
approximately  200  meters  longer  than  the  true  slant  range. 

Tables  4.3  and  4.4  present  statistics  for  Figures  4.6  and  4.7  respectively.  As 
noted  for  the  outdoor  range  error  analysis,  the  bias  present  in  each  static  collect  is 
different.  RF  interference  introduces  the  varying  range  error  bias.  The  RMS  error 
presents  a  measure  of  RF  interference  and  uncertainty  of  the  range  measurements. 
The  lower  level  of  RF  interference  present  for  radio  pair  1  radio  pair  is  evident  by 
the  lower  RMS  error  of  23.8  meters.  Likewise,  the  161.1  meter  RMS  error  for  radio 
pair  2  is  higher  due  to  signihcant  interference.  In  both  cases,  the  Chi-Squared  test 
result  is  non- Gaussian.  Both  radio  pairs  for  the  north  location  do  not  conform  to  the 
Gaussian  distribution.  This  means  the  white,  Gaussian  noise  assumption  used  in  the 
EKF  range  update  model  is  not  valid. 

Figures  4.8  and  4.9  along  with  Tables  4.5  and  4.6  present  the  histograms  and 
statistics  for  both  radio  pairs  at  the  south  hallway  location.  Upon  initial  inspection 
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Range  Error  [m] 


Figure  4.6:  Indoor  North  Range  Error  Between  Radio  Pair  1.  Note  the  long  tail  on 
the  range  error  distribution  due  to  RF  interference. 


Histogram  and  Gaussian  Approximation  of  Range  Error  (North,  Radio  Pair  2) 


Figure  4.7:  Indoor  North  Range  Error  Between  Radio  Pair  2.  Note  the  extreme 
range  errors  due  to  RF  signal  interference. 
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Table  4.3:  Statistics  for  North,  Radio  Pair  1  Range  Error.  The  range  distribution 
has  a  positive  mean,  due  to  RF  interference,  and  does  not  conform  to  the  Gaussian 
distribution. 


Parameter 

Value 

Mean 

19.56  m 

Std  Dev 

13.52  m 

RMS  Error 

23.77  m 

Ghi  Squared  Test 

Non-Gaussian 

Table  4.4:  Statistics  for  North,  Radio  Pair  2  Range  Error.  The  range  distribution 
has  a  large  positive  mean,  due  to  RF  interference,  and  does  not  conform  to  the 
Gaussian  distribution. 


Parameter 

Value 

Mean 

113.78  m 

Std  Dev 

114.10  m 

RMS  Error 

161.13  m 

Ghi  Squared  Test 

Non-Gaussian 

of  both  graphs,  the  spread  of  the  ranges  do  not  exceed  120  meters  as  was  present 
in  Figure  4.7.  This  indicates  less  RF  interference.  The  bias  of  the  error  is  again 
different  for  both  of  the  south  radio  pairs.  The  bias  of  20.5  meters  and  RMS  error 
of  24.4  meters  for  the  south  radio  pair  2  is  similar  to  the  north  radio  pair  1  which 
suggests  similar  RF  interference  exists  between  these  radio  pairs  at  their  respective 
locations.  A  difference  from  the  north  radio  pair  1  is  the  outcome  of  the  Ghi-Square 
GOF  test.  The  Ghi  Squared  test  for  south  radio  pair  2  reports  the  distribution  is 
Gaussian.  This  indicates  that,  under  certain  circumstances,  the  distribution  of  range 
error  may  be  considered  a  normal  distribution.  However,  the  lack  of  conformity  to 
the  normal  distribution  for  a  majority  of  the  range  error  histograms  suggests  the  bulk 
of  range  errors  do  not  £t  a  Gaussian  distribution. 

Much  greater  range  error  is  present  in  the  indoor  stationary  range  measurements 
as  compared  with  the  outdoor  stationary  range  measurements.  The  RF  channel  ob¬ 
structions  which  induce  RF  interference  and  signal  attenuation,  cause  a  significant 
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Histogram  and  Gaussian  Approximation  of  Range  Error  (South,  Radio  Pair  1) 


Range  Error  [m] 


Figure  4.8:  Indoor  South  Range  Error  Between  Radio  Pair  1.  Note  the  positive  tail 
on  distribution  due  to  RF  interference. 


Histogram  and  Gaussian  Approximation  of  Range  Error  (South,  Radio  Pair  2) 
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Figure  4.9:  Indoor  South  Range  Error  Between  Radio  Pair  2.  Note  the  close  £t  of 
the  histogram  to  the  normal  distribution  overlay. 
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Table  4.5:  Statistics  for  South,  Radio  Pair  1  Range  Error.  The  range  distribution 
has  a  positive  mean,  due  to  RF  interference,  and  does  not  conform  to  the  Gaussian 
distribution. 


Parameter 

Value 

Mean 

34.80  m 

Std  Dev 

27.34  m 

RMS  Error 

44.26  m 

Ghi  Squared  Test 

Non-Gaussian 

Table  4.6:  Statistics  for  South,  Radio  Pair  2  Range  Error.  The  range  distribution 
has  a  positive  mean,  due  to  RF  interference,  and  does  not  conform  to  the  Gaussian 
distribution. 


Parameter 

Value 

Mean 

20.46  m 

Std  Dev 

13.21  m 

RMS  Error 

24.35  m 

Ghi  Squared  Test 

Gaussian 

increase  in  range  error  for  the  indoor  usage  of  the  DH500  radio  system.  The  model 
of  true  slant  range  plus  white,  Gaussian  noise  used  in  the  EKE  radio  range  model 
must  be  adjusted  to  properly  account  for  the  error  in  the  measured  range  for  both 
the  indoor  and  outdoor  applications. 

Possible  modihcations  to  the  EKE  range  update  algorithm  include  increasing 
the  noise  or  uncertainty  of  the  range  measurement  model.  An  increase  in  the  range 
model  uncertainty  will  reduce  the  effectiveness  of  all  range  measurements  available  to 
the  hlter  without  regard  for  range  measurements  with  low  error.  From  the  histograms 
presented  in  the  outdoor  and  indoor  range  error  characterization,  some  range  mea¬ 
surements  have  relatively  low  error  compared  with  the  rest.  It  would  be  preferable  to 
simply  remove  the  erroneous  range  measurements  and  reduce  the  EKE  range  model 
uncertainty. 

Residual  monitoring,  presented  in  Section  2.9.1,  allows  the  rejection  of  a  particu¬ 
lar  range  measurement  based  on  its  residual.  When  the  residual  exceeds  the  threshold 
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the  measurement  is  rejected.  A  typical  threshold  is  3-a,  where  a  is  the  standard  devi¬ 
ation  of  the  residual  uncertainty.  This  algorithm  ceases  to  work  constructively  when 
the  internal  navigation  solution  of  the  navigation  system  drifts  so  far  that  every  radio 
residual  measurement  error  is  greater  than  the  threshold,  resulting  in  the  rejection  of 
the  all  range  measurements. 

A  better  approach  is  to  vary  the  range  model  uncertainty  based  on  the  error 
present  in  each  range  measurement.  The  Sage-Husa  adaptive  algorithm  provides  this 
solution.  Range  measurements  with  lower  error  more  strongly  correct  the  EKF  po¬ 
sition  states,  while  ranges  with  large  error  do  not  signihcantly  corrupt  the  position 
estimate.  Another  beneht  is  observed  when  the  navigation  solution  contains  large  po¬ 
sition  error.  Instead  of  rejecting  all  range  measurements  as  with  residual  monitoring, 
the  Sage-Husa  adaptive  algorithm  includes  all  the  available  range  measurements  to 
help  correct  and  reduce  the  navigation  system  position  error.  The  next  two  sections 
present  analysis  of  the  moving  data  set.  The  RMS  range  errors  presented  for  the 
stationary  data  sets  are  used  to  initialize  the  EKF  range  model  uncertainties  in  the 
next  sections. 

4-2  Outdoor  Moving  Analysis 

The  results  from  the  moving  outdoor  data  collections  are  presented  in  this  sec¬ 
tion.  Setup  for  the  outdoor  moving  data  collections  is  presented  in  Section  3.2.2. 
Four  outdoor  moving  data  collections  are  performed.  The  four  data  collections  are 
labeled  “moving.la,”  “moving.lb,”  “moving_2a,”  and  “moving_2b.”  The  duration  of 
each  collect  is  laid  out  in  Table  4.7. 

Table  4.7:  Total  Stationary  and  Moving  time  of  the  Outdoor  Moving  Data  Sets 


Data  Collection 

Total  Time 

moving_la 

595  s 

moving.lb 

559  s 

moving_2a 

551  s 

moving_2b 

559  s 
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Post-processing  of  the  four  data  sets  is  conducted  using  a  bottom-up  tuning 
approach.  First,  the  four  data  collections  are  post-processed  using  only  INS  data 
with  all  hlter  bias-estimation  disabled.  This  helps  emphasize  the  impact  of  the  hlter 
bias  estimation  algorithm  which  is  enabled  next.  Then,  radio  range  data  along  with 
the  simulated  baro-altitude  data  is  added.  Tuning  of  the  EKF  radio  range  model 
uncertainty  is  performed  at  this  step.  Finally,  the  results  from  EKF  radio  range  model 
with  residual  monitoring  and  then  the  Sage-Husa  adaptive  algorithm  are  presented 
to  evaluate  the  impact  of  these  algorithms  on  further  constraining  the  INS  drift. 

Of  the  four  outdoor  moving  data  sets,  “moving.lb”  and  “moving_2b”  are  cho¬ 
sen  to  provide  the  reader  detailed  examples  for  each  step  of  the  bottom-up  tuning 
approach.  Horizontal  trajectory  comparison  for  remaining  data  sets  are  analyzed 
together  at  the  end  of  each  subsection  to  show  the  variety  of  results  obtained. 

During  the  collection  of  each  of  the  four  data  sets,  the  “true  trajectory”  of  the 
navigation  system  is  also  recorded.  This  true  trajectory  consists  of  the  INS-aided 
DGPS  position,  velocity,  and  attitude  estimate  from  the  SPAN. 

4.2.1  INS  Data  Only,  No  Filter  Bias  Estimation.  The  first  iteration  of 
post-processing  uses  only  INS  data.  No  hlter  alignment  is  performed  because  the 
hlter  bias  states  are  disabled  for  this  hrst  iteration.  EKF  post-processing  alignment 
is  separate  from  the  SPAN  HG1700  INS  alignment  procedures  conducted  as  stated 
in  Section  3.1.1.  The  EKF’s  bias  estimation  is  also  disabled  for  this  hrst  iteration  to 
display  the  drift  present  in  pure  inertial  navigation.  This  strongly  motivates  the  need 
for  drift  constraint  capability  within  the  navigation  system.  The  following  hgures 
present  the  position  and  attitude  error  accumulated  by  the  navigation  system. 

Figure  4.10  presents  the  position  error  graph.  The  1-a  hlter  uncertainty  bounds 
show  the  EKF’s  uncertainty  for  each  state.  The  hlter  has  a  very  high  level  of  un¬ 
certainty  in  the  position  states  after  500-1-  seconds  of  operation.  This  is  typical  for 
a  tactical-grade  INS  system,  but  provides  unusable  long-term  navigation  capability. 
Next,  the  attitude  errors  are  evaluated. 
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Filter  Position  Error  &  Uncertainty 


Figure  4.10:  Position  Error  for  Data  Collect  “moving_lb.”  Only  INS  data  is  used 
in  the  filter  and  the  bias  estimation  capability  is  disabled.  Observe  the  very  high  hlter 
position  uncertainty  which  exceeds  10,0000  meters.  This  is  typical  for  a  tactical-grade 
INS,  but  does  not  provide  usable  long-term  navigation  capability. 
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Filter  Attitude  Error  &  Uncertainty 


Relative  GPS  Time  (s) 

Figure  4.11:  Attitude  Error  for  Data  Collect  “moving_lb.”  Only  INS  data  is  used 
in  the  filter  and  the  bias  estimation  capability  is  disabled.  Note  the  large  attitude 
uncertainty  of  87  mili-radians  (±5.0°). 

Figure  4.11  presents  the  attitude  error  graph.  As  with  the  position  error  graph, 
the  filter  attitude  errors  fall  well  within  the  1-a  filter  uncertainty  bounds.  The  filter 
also  has  a  large  level  of  uncertainty  for  each  of  the  attitude  states.  The  uncertainty 
of  87  mili-radians  correlates  to  ±5.0°  of  attitude  error.  This  is  characteristic  of  a 
tactical-rade  INS,  but  presents  unusable  navigation  capability  for  the  purposes  of  this 
thesis.  The  brief  increase  in  attitude  error  observed  between  approximately  150  and 
250  seconds  is  an  anomaly  potentially  due  to  a  grounding  issue  within  the  naviga¬ 
tion  system.  The  time  constraints  of  this  thesis  prevented  the  source  of  this  anomaly 
from  being  found.  This  anomaly  is  found  throughout  the  outdoor  moving  data  col¬ 
lections  presented.  The  effect  of  position  uncertainty  on  the  horizontal  trajectory  of 
the  navigation  system  is  presented  in  the  next  set  of  graphs. 

Figure  4.12  presents  the  EKF  post-processed  results  for  the  four  outside  data 
runs.  Each  post-processed  trajectory  is  compared  against  the  true  trajectory  as  pro¬ 
vided  by  the  SPAN  INS-aided  DGPS  position  solution.  Note  how  the  horizontal 
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Horizontal  Trajectory  Comparison  {Local-Level  NED) 


Horizontal  Trajectory  Comparison  {Local-Level  NED) 


Horizontal  Trajectory  Comparison  {Local-Level  NED) 


Horizontal  Trajectory  Comparison  {Local-Level  NED) 


Figure  4.12:  INS  Data  only  Horizontal  Trajectory  Comparison  for  the  four  Outdoor 
Moving  Data  Collections.  Note  the  variety  in  the  amount  of  drift  for  each  data  set. 

position  errors  range  upwards  of  2000  meters  for  Figure  12(c).  The  difference  be¬ 
tween  each  of  the  four  plots  in  Figure  4.12  is  primarily  due  to  noise  as  described  in 
Section  2.4.3. 

The  signihcant  trajectory  drift  observed  in  Figure  4.12  strongly  motivates  the 
need  for  navigation  sensor  with  constrained,  long-term  drift.  Before  such  a  sensor  is 
added,  EKF  bias  estimation  is  enabled  to  extract  the  best  possible  navigation  using 
only  INS  data. 
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Figure  4.13:  Position  Error  for  Data  Collection  “moving_lb.”  The  EKF  uses  only 
INS  data,  but  with  bias  estimation  enabled.  Note  the  factor  of  100  reduction  in 
hlter  uncertainty  for  the  position  stated  as  compared  with  Figure  4.10  where  bias 
estimation  is  disabled. 


J^.2.2  Enable  Filter  Bias  Estimation.  To  allow  the  EKF  to  properly  estimate 
the  bias  states  for  the  accelerometers  and  gyroscopes,  a  2-minute  stationary  alignment 
is  performed.  This  alignment  is  performed  by  providing  measurement  updates  to  the 
EKF  of  position,  velocity,  and  attitude  using  the  INS-aided  DGPS  truth  navigation 
solution.  After  the  2-minute  alignment,  the  truth  data  updates  are  removed  and 
the  EKF  continues  to  estimate  the  navigation  states.  Only  INS  data  is  used  for  the 
following  analysis. 

Figure  4.13  presents  the  position  error  graph.  The  l-cr  bounds  reflect  several 
orders  of  magnitude  reduction  in  the  hlter  uncertainty  afforded  by  estimation  of  the 
INS  biases.  Note  also  that  the  vertical  position  channel  error  is  also  reduced  by  a 
factor  of  10. 

Figure  4.14  presents  the  attitude  error  graph.  Note  how  strongly  the  attitude 
error  is  constrained.  The  hlter  attitude  uncertainty  is  reduced  by  a  magnitude  of  100 
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Filter  Attitude  Error  &  Uncertainty 


Figure  4.14:  Attitude  Error  for  Data  Collect  “moving_lb.”  The  EKF  uses  only  INS 
data,  but  with  bias  estimation  enabled.  Note  the  factor  of  100  decrease  in  hlter  un¬ 
certainty  for  the  attitude  states  when  compared  with  no  bias  estimation  as  presented 
in  Figure  4.11. 


as  compared  with  Figure  4.11  in  the  previous  section.  The  attitude  anomaly  is  much 
more  apparent  due  to  the  reduction  in  scale  of  the  vertical  axes.  Besides  the  anomaly, 
the  filter’s  attitude  error  remains  within  the  filter’s  1-cr  uncertainty  bounds.  The  next 
two  graphs  present  the  hlter’s  estimate  of  the  gyro  and  accelerometer  biases. 

Figure  4.15  presents  the  accelerometer  bias  estimated  by  the  EKF.  The  2-minute 
alignment  allows  the  filter  to  estimate  the  accelerometer  biases  with  low  uncertainty. 
From  Figure  4.15,  the  biases  estimated  are  quite  small,  but  the  horizontal  trajectory 
comparison  graphs  presented  at  the  end  of  this  section  displays  the  signihcant  drift 
reduction  afforded  by  removal  of  these  small  biases.  The  hlter  uncertainty  of  the 
accelerometer  biases  reached  a  minimum  around  the  120  second  point  where  the 
alignment  updates  are  removed  from  the  hlter.  The  hlter’s  estimation  of  the  biases 
drifts  slightly  over  the  remaining  trajectory  time.  The  hlter’s  uncertainty  for  each 
accelerometer  axis  also  grows  over  time.  This  growth  is  expected.  After  the  alignment 


83 


Accelerometer  Bias  Estimation 


GPS  Time  (s) 

Figure  4.15:  Accelerometer  Bias  Estimation  for  Data  Collection  “moving_lb.”  The 
filter  is  given  only  INS  data  with  bias  estimation  enabled.  The  low  uncertainty  of  the 
bias  estimate  is  due  to  the  length  of  the  hlter  alignment. 

updates  are  removed,  the  hlter  is  not  able  to  accurately  observe  the  accelerometer 
biases  which  results  in  the  increasing  uncertainty  of  the  hlter’s  accelerometer  bias 
estimates. 

Similarly  for  the  gyroscope  bias  estimation,  as  presented  in  Figure  4.16,  the  EKF 
is  able  to  accurately  estimate  the  gyro  biases  within  the  2-minute  initial  alignment. 
As  observed  from  Figure  4.16,  the  gyro  biases  vary  until  the  2  minute  mark  when 
alignment  updates  are  removed.  From  this  point  the  hlter  does  not  signihcantly 
change  the  gyro  bias  estimates.  A  potential  pitfall  of  the  gyro  bias  estimation  is  the 
continual  decrease  of  hlter  uncertainty  as  is  observed  for  the  gyro  bias  channel. 
As  the  hlter’s  uncertainty  decreases,  the  weight  placed  on  the  incoming  sensor  data 
decreases  resulting  in  potential  hlter  divergence  for  the  particular  state.  Careful  hlter 
tuning  helps  to  alleviate  the  divergence  issue.  The  next  set  of  plots  display  the  overall 
strength  of  EKF  bias  estimation  in  reducing  trajectory  drift. 
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Gyroscope  Bias  Estimation 
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Figure  4.16:  Gyroscope  Bias  Estimation  for  Data  Collection  “moving_lb.”  The  filter 
is  given  only  INS  data  with  bias  estimation  is  enabled.  The  low  uncertainty  of  the 
bias  estimate  is  due  to  the  length  of  the  hlter  alignment. 


The  effect  of  EKF  bias  estimation  on  each  of  the  four  outdoor  moving  data 
collections  is  observed  in  Figure  4.17.  In  general,  estimation  of  the  accelerometer  and 
gyro  biases  greatly  improves  the  INS  navigation  solution.  The  continued  poor  results 
from  data  collection  “moving_2b”  are  due  to  attitude  anomalies,  similar  to  Figure  4.14, 
which  occur  for  the  duration  of  the  moving  portion  of  the  trajectory.  The  orientation 
of  the  INS  accelerometers  is  derived  from  the  gyroscope  attitude  estimate.  Errors 
in  the  attitude  estimate  due  to  the  anomalies  cause  signihcant  errors  in  the  position 
estimate. 

The  navigation  system’s  trajectory  drift  has  been  signihcantly  reduced  by  EKF 
bias  estimation.  The  next  section  adds  radio  range  measurements  and  vertical  channel 
constraint  to  the  EKF  post-processing. 


4.2.3  Add  Vertical  Channel  Constraint  and  Range  Measurements.  The  hlter 
bias  estimation  added  in  the  previous  section  resulted  in  a  signihcant  reduction  in  the 
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Horizontal  Trajectory  Comparison  (Locai-Level  NED) 


Horizontal  Trajectory  Comparison  {Locai-Levei  NED) 


Horizontal  Trajectory  Comparison  {Local-Level  NED) 
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Figure  4.17:  INS  Data  with  Bias  Estimation  Horizontal  Trajectory  Comparison. 

Significant  reduction  in  trajectory  drift  is  observed  with  EKF  bias  estimation  enabled. 
The  significant  drift  in  data  collect  “moving_2b”  results  from  large  attitude  anomalies 
which  add  to  the  accumulation  of  INS  attitude  error. 


Radio  Range  Residual  Analysis 


600 


Figure  4.18:  Range  Residual  Analysis  for  Data  Collect  “moving_lb.”  This  graphs 
shows  the  initial  choice  of  8  meters  for  the  EKF  radio  range  model  uncertainty  is  too 
low.  The  actual  uncertainty  of  the  range  measurements  is  higher. 

trajectory  drift.  This  section  evaluates  the  addition  of  vertical  channel  constraint 
data  and  radio  range  data  to  the  EKF.  The  post-processing  now  consists  of  INS  data, 
vertical  constraint  data,  radio  range  measurements,  and  EKF  bias  estimation.  As 
mentioned  in  Chapter  III,  the  vertical  channel  constraint  data  consists  of  corrupted 
vertical  channel  data  from  the  INS-aided  DGPS  truth  data. 

Based  on  the  outdoor  stationary  data  collection  presented  in  Section  4.1.1,  an 
initial  EKF  radio  range  model  uncertainty  of  8  meters  is  chosen.  Figure  4.18  shows 
the  range  residual  analysis  for  this  model  uncertainty  value.  The  range  residuals  are 
much  larger  than  the  l-cx  standard  deviation  bounds  as  indicated  by  the  residual 
covariance.  After  analysis  of  the  range  residuals  for  each  of  the  four  outdoor  moving 
data  collections,  the  model  uncertainty  is  redehned  as  55  meters. 

Residual  analysis  with  the  update  model  uncertainty  is  shown  in  Figure  4.19. 
The  radio  range  model  uncertainty  is  now  correctly  tuned  for  the  outdoor  environ- 
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ment.  It  will  be  shown  later  that  this  uncertainty  must  be  re-tuned  for  the  indoor 
environment.  The  hlter  now  has  an  accurate  range  model  as  observed  by  70-80%  of 
range  residuals  now  falling  within  the  1-a  residual  covariance  bounds. 

The  effect  of  the  range  measurements  and  vertical  channel  constraint  data  as 
viewed  from  the  error  in  the  hlter  position  states  is  shown  in  Figure  4.20.  Note 
the  2-minute  hlter  alignment  procedure  which  greately  constrains  the  position  states. 
Observe  the  constrained  vertical  channel  which  results  from  incorporating  slighly 
corrupted  INS-aided  DGPS  vertical  channel  measurements  into  the  EKF.  Also  observe 
the  constraint  on  the  horizontal  navigation  channels  and  Pg.  This  displays  the 
ehect  of  the  EKF  radio  range  model  on  the  hlter  uncertainty  and  the  ehect  of  the 
range  measurements  on  the  hlter  position  error.  Both  the  uncertainty  and  position 
error  are  constrained  as  dehned  by  the  radio  range  model.  The  position  estimation 
errors  committed  by  the  EKF  match  the  hlter’s  uncertainty.  This  indicates  the  hlter 
is  tuned  correctly. 

Note  the  higher  overall  uncertainty  shown  for  the  Pg  east-axis  in  Figure  4.20. 
The  increased  uncertainty  is  due  to  the  geometry  of  the  radio  positions.  Given  the 
radio  positions  described  in  Section  3.2.2,  the  radio  system  provides  much  stronger 
observation  of  the  north-axis  movement  of  the  sensor  platform  then  the  east-axis 
movement.  This  is  rehected  in  the  increased  Kalman  hlter  state  uncertainty  for  the 
Pg  east-axis.  The  next  set  of  plots  show  the  drift  constraint  capability  of  the  radio 
range  measurements. 

Figure  4.21  displays  the  ehect  of  adding  the  radio  range  measurements  and 
vertical  channel  constraint  data  to  the  EKF  post-processing.  Overall,  the  radio  range 
data  provides  signihcant  trajectory  drift  constraint.  The  most  notable  example  is 
in  Figure  4.21  (d)  which  has  large  trajectory  drift  due  to  signihcant  attitude  error 
anomalies.  The  radio  range  data  helps  constrain  the  trajectory  to  within  20-50  meters 
of  the  trajectory  truth  data. 


Radio  Range  Residual  Analysis 


Figure  4.19:  Range  Residual  Analysis  for  Data  Collect  “moving_lb.”  Re-tuned 

EKF  radio  range  model  with  uncertainty  defined  as  55  meters.  The  updated  model 
uncertainty  now  hts  the  actual  range  measurement  data  as  is  shown  by  70-80%  of  the 
range  residuals  falling  within  the  1-a  residual  covariance  bounds. 


Filter  Position  Error  &  Uncertainty 


Figure  4.20:  Position  Error  for  Data  Collect  “moving_lb.”  The  EKF  utilizes  bias 
estimation  with  INS,  radio  range,  and  vertical  channel  constraint  data.  The  radio 
model  uncertainty  has  been  re-tuned  to  55  meters.  Note  the  constrained  uncertainty 
for  the  filter  position  states.  Also,  note  the  filter  position  errors  do  not  significantly 
exceed  the  l-cx  filter  uncertainty  bounds,  indicating  a  correctly  tuned  filter. 
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Horizontal  Trajectory  Comparison  {Local-Level  NED)  Horizontal  Trajectory  Comparison  (Local-Level  NED) 


(a)  Moving  la 


(b)  Moving  lb 
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Horizontal  Trajectory  Comparison  (Local-Level  NED) 


(c)  Moving  2a 


(d)  Moving  2b 


Figure  4.21:  Horizontal  Trajectory  Comparison.  Range  measurements  and  vertical 
channel  constraint  data  are  added  to  the  INS  data  with  EKF  bias  estimation  enabled. 
Overall,  the  additional  data  provides  notable  trajectory  drift  constraint. 
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Better  trajectory  drift  constraint  performance  is  observed  for  Figures  4.21  (a) 
and  4.21  (b).  This  is  largely  due  to  the  fewer  attitude  error  anomalies.  The  trajectory 
error  is  constrained  well  within  15-50  meter  bounds. 

The  radio  range  measurements  actually  corrupt  the  INS-|-Bias  trajectory  as 
shown  in  Figure  4.21  (c).  The  INS-only  with  hlter  bias  estimation  trajectory  has  very 
low  drift.  The  INS-|-Bias  trajectory  drifts  most  signihcantly  at  the  end  of  the  trajec¬ 
tory  where  the  INS-only  trajectory  drifts  to  about  100  meters  north  of  the  true  end 
of  the  trajectory.  The  very  low  INS-only  drift  is  due  to  good  initial  hlter  alignment. 
The  notable  error  added  from  the  range  measurements  is  due  to  the  tuning  of  the  INS 
model  and  range  model  uncertainties.  The  hlter  position  uncertainty  increases  over 
time  with  only  INS  data  as  shown  previously  in  Figure  4.10  and  4.13.  Bias  estimation 
helps  to  reduce  the  magnitude  of  the  growth  in  hlter  position  uncertainty,  but  bias 
estimation  does  not  constrain  the  long-term  increase  of  this  uncertainty.  The  large 
hlter  uncertainty  allows  the  range  measurements  to  corrupt  the  horizontal  position 
states. 

Observing  the  four  horizontal  trajectory  comparison  plots,  the  very  low  drift 
observed  in  Figure  4.21  (c)  is  not  particularly  common.  The  Figures  4.21  (a)  and 
4.21  (b)  show  INS-only  drift  consistent  with  a  tactical-grade  INS.  The  signihcant 
INS-|-Bias  drift  shown  in  Figure  4.21  (d)  is  due  largely  to  poor  initial  hlter  align¬ 
ment.  A  potential  source  of  the  poor  initial  alignment  comes  from  slight  movement 
of  navigation  system  during  the  alignment  procedure. 

Now  that  the  outdoor  drift-constraint  capability  of  the  DH500  radio  system 
has  been  demonstrated,  two  algorithms  are  applied  to  the  EKF  range  measurement 
update  algorithm  to  further  constraint  the  trajectory  drift.  The  next  section  presents 
results  obtained  from  residual  monitoring  and  the  Sage-Husa  adaptive  algorithm. 

4-2.4  Residual  Monitoring  and  Sage-Husa.  The  analysis  of  residual  mon¬ 
itoring  and  the  Sage-Husa  adaptive  algorithm  are  presented  in  this  section.  The 
inclusion  of  range  measurements  and  vertical  channel  constraint  data  in  the  previous 
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section  showed  improvement  for  a  majority  of  the  outdoor  data  runs.  The  trajec¬ 
tory  corruption  noted  in  Figure  4.21  (c)  is  due  to  the  hlter  alignment  as  discussed  in 
the  previous  section.  To  help  reduce  the  trajectory  corruption  and  further  constrain 
trajectory  drift,  the  EKF  radio  range  model  is  modihed  to  include  hrst,  residual  mon¬ 
itoring  (Section  2.9.1)  and  then  the  Sage-Huse  adaptive  algorithm  (Section  2.9.2). 
These  algorithms  are  implemented  separately.  The  section  continues  with  analysis  of 
the  residual  monitoring  technique. 

4.2.4- 1  Residual  Monitoring.  Of  the  four  outdoor  moving  data  collec¬ 
tions,  data  set  “moving_2b”  is  the  only  one  which  residual  monitoring  significantly 
impacts.  Data  set  “moving_2b”  contains  residual  covariances  which  exceed  the  3-a 
threshold.  The  range  residuals  for  the  other  three  data  sets  remain  below  the  3-a 
threshold.  Figure  4.22  displays  the  range  residuals  for  “moving_2b”  before  residual 
monitoring  is  applied.  Note  the  large  residuals  that  occur  for  the  second  radio  pair 
towards  the  beginning  of  the  data  run.  These  large  residuals  indicate  that  these  range 
measurements  do  not  conform  to  the  EKF  range  model.  The  horizontal  trajectory 
comparison  graph  shown  at  the  end  of  this  section  displays  the  additional  trajectory 
error  caused  by  these  erroneous  range  measurements.  Residual  monitoring  rejects 
such  measurements,  eliminating  their  negative  impact  on  the  horizontal  trajectory. 

Once  residual  monitoring  is  applied,  the  erroneous  range  measurements  are  re¬ 
jected.  Figure  4.23  presents  the  range  residuals  for  data  set  “moving_2b”  after  the 
residual  monitoring  algorithm  is  applied.  The  erroneous  range  measurements  are  now 
rejected  and  are  not  used  to  update  the  hlter  position  state  estimates.  Note  that 
erroneous  range  measurements  do  not  occur  for  the  hrst  radio  pair,  but  the  residuals 
have  changed  slightly.  This  change  is  due  to  the  variation  in  vehicle  state  estimation 
by  the  EKF  due  to  the  removal  of  erroneous  range  measurements  in  the  second  ra¬ 
dio  pair.  Residual  monitoring  is  swapped  for  the  Sage-Husa  algorithm  for  the  next 
portion  of  analysis. 
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Radio  Range  Residual  Analysis 


Figure  4.22:  Range  Residual  Analysis  for  Data  Collect  “moving_2b.”  This  graph 
shows  the  range  residuals  before  residual  monitoring  is  applied.  Note  the  large  neg¬ 
ative  range  residuals  which  occur  towards  the  beginning  of  the  trajectory  for  radio 
pair  2. 
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Radio  Range  Residual  Analysis 


Figure  4.23:  Range  Residual  Analysis  for  Data  Collection  “moving_2b.”  The  graph 
shows  the  range  residuals  that  are  rejected  by  the  residual  monitoring  algorithm. 
These  residuals  exceeded  the  3-ct  threshold  and  are  not  use  to  update  the  EKF’s 
position  states. 
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4-2. 4-2  Sage-Husa.  The  Sage-Husa  adaptive  algorithm  requires  tuning 
of  the  memory  factor  d.  As  discussed  in  Section  2.9.2,  larger  memory  factors  place 
more  weight  on  the  current  parameter  estimate  and  less  weight  on  the  time-average 
history  of  the  parameter.  The  performance  of  the  Sage-Husa  algorithm  is  dependent 
on  both  the  memory  factor  d  and  the  uncertainty  of  the  measurements  in  each  data 
set. 

Figure  4.24  presents  the  average  RMS  trajectory  error  of  each  outdoor  moving 
data  set  for  a  range  of  Sage-Husa  memory  factors.  Differences  in  the  error  for  each 
range  measurement  and  the  amount  of  trajectory  drift  present  in  the  INS  account  for 
the  variety  of  trajectory  errors.  Sage-Husa  memory  factors  above  0.10  allowed  the 
range  model  uncertainty  estimate  to  become  negative  for  certain  data  runs  causing 
EKF  instability.  A  memory  factor  of  0.005  is  chosen  for  the  four  outdoor  moving 
data  sets  as  a  compromise  to  obtain  RMS  trajectory  error  below  that  of  the  standard 
range  update  algorithm.  While  this  tuning  methodology  provided  a  decrease  in  overall 
trajectory  error  for  all  four  of  the  the  outdoor  moving  data  sets,  a  more  robust  tuning 
strategy  would  tune  off  of  a  subset  of  the  outdoor  data  sets  and  then  evaluate  this 
memory  factor  on  the  remaining  data  sets.  The  Sage-Husa  algorithm  is  potentially 
over-tuned  from  the  tuning  methodology  applied. 

Figure  4.25  presents  the  estimated  value  of  the  mean  and  variance  for  the  EKF 
radio  range  model  noise.  The  estimated  mean  directly  correlates  to  the  bias  in  the 
range  measurements.  The  estimated  variance  directly  correlates  with  the  EKF  radio 
range  model  uncertainty  and  is  presented  with  l-a  standard  deviation  bounds.  The 
range  residuals  are  also  included  in  the  plot  to  provide  a  reference  for  changes  in  the 
estimated  mean  and  variance. 

The  mean  and  variance  are  initialized  at  zero  and  (55m) ^  respectively.  Once 
the  EKF  begins  post-processing,  the  Sage-Husa  algorithm  uses  the  range  residuals  to 
provide  an  estimate  of  the  bias  and  model  uncertainty.  For  radio  pair  1,  the  estimated 
EKF  range  model  uncertainty  decreases  over  the  entire  length  of  the  run.  This  is  due 
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Compare  Sage-Husa  Memory  Factors 


Compare  Sage-Husa  Memory  Factors 


(a)  Moving  la  (b)  Moving  lb 


Compare  Sage-Husa  Memory  Factors  Compare  Sage-Husa  Memory  Factors 


(c)  Moving  2a  (d)  Moving  2b 


Figure  4.24:  Average  RMS  Trajectory  Error  for  a  Range  of  Sage-Husa  Memory 

Factors.  Note  how  each  data  set  responds  differently  to  the  same  range  of  memory 
factors.  Average  RMS  trajectory  error  for  the  standard  EKF  range  update  algorithm 
is  shown  for  comparison. 
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Sage-Husa  Adaptive  Algorithm:  Estimation  Analysis 
lOOr .  .  .  . 
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Figure  4.25:  Sage-Husa  Estimate  Analysis  for  Data  Collection  “moving_2b.”  Note 
how  the  measurement  noise  variance,  or  model  uncertainty,  adjusts  to  account  for 
changes  in  the  residual  distribution.  The  measurement  noise  mean,  or  range  bias, 
slowly  tracks  with  the  average  range  residual  value  providing  an  estimate  of  the  bias 
in  the  range  measurements. 


to  the  range  measurements  for  this  particular  data  set  having  an  average  uncertainty 
lower  than  the  55  meters  dehned  the  the  EKF  radio  range  model.  The  measurement 
bias  trends  slightly  upward  in  the  middle  of  the  data  set  and  then  downward  towards 
the  end  of  the  data  set.  Overall  the  bias  remains  right  around  zero.  This  is  expected 
due  to  the  minimal  RF  interference  present  in  the  outdoor  environment. 

For  radio  pair  2,  the  large  negative  range  residuals  at  the  beginning  of  the 
trajectory  cause  the  estimated  uncertainty  for  this  radio  pair  to  strongly  increase. 
The  increase  in  model  uncertainty  causes  the  EKF  to  place  less  weight  on  the  range 
measurements,  reducing  the  effect  of  poor  range  measurements  on  the  estimated  tra¬ 
jectory.  The  bias  estimation  becomes  negative  to  try  to  account  for  the  large  negative 
error  in  the  initial  residuals.  The  remainder  of  the  range  residuals  are  distributed 
fairly  evenly  around  the  zero  residual  horizontal  axis.  The  approximate  zero-mean 
distribution  of  these  residuals  and  the  low  memory  factor  of  0.005  causes  the  bias 
estimation  to  remaining  negative  for  the  remainder  of  the  trajectory.  The  estimate 
of  the  model  uncertainty  for  radio  pair  2  decreases  over  the  latter  portion  of  the  data 
run  due  to  the  range  residuals  clustering  around  the  zero-residual  horizontal  axis. 

The  measurement  bias  and  model  uncertainty  estimated  by  the  Sage-Husa  algo¬ 
rithm  are  used  to  correct  the  range  measurements  provided  to  the  EKF.  Figure  4.26 
presents  the  post-processed  trajectory  results  for  the  basic  EKF  radio  range  update 
algorithm,  labeled  “Standard” ,  the  residual  monitoring  algorithm,  labeled  “Residual 
Mon”,  and  the  Sage-Husa  adaptive  algorithm.  Analysis  continues  with  discussion  of 
residual  monitoring  results. 

As  stated  previously  in  this  section,  residual  monitoring  only  effected  the  “mov- 
ing_2b”  data  collection  due  to  large  range  residuals.  The  range  residuals  for  the 
remaining  data  collections  do  not  exceed  the  threshold,  which  resulted  in  no  change 
in  the  constraint  of  trajectory  drift.  This  is  observed  by  the  complete  overlap  of 
the  standard  and  residual  monitoring  plots  shown  in  Figures  4.26  (a),  4.26  (b),  and 
4.26  (c).  Residual  monitoring  did  have  an  effect  of  reducing  the  range  measurement 
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Horizontal  Trajectory  Comparison  {Locai-Levei  NED) 


Horizontal  Trajectory  Comparison  (Locai-Levei  NED) 


(a)  Moving  la 

Horizontal  Trajectory  Comparison  (Locai-Levei  NED) 


(b)  Moving  lb 

Horizontal  Trajectory  Comparison  (Locai-Levei  NED) 


(c)  Moving  2a 


(d)  Moving  2b 


Figure  4.26:  Residual  Monitoring  and  Sage-Husa  Horizontal  Trajectory  Compari¬ 
son.  Residual  monitoring  only  impacts  data  set  “moving_2b”  due  to  the  large  range 
residuals.  The  Sage-Husa  adaptive  algorithm  has  a  variety  of  results  dependent  on 
the  amount  of  INS  drift  and  quality  of  range  measurements. 
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trajectory  corruption  on  the  fourth  data  collection,  shown  in  Figure  4.26  (d).  This 
is  evident  by  the  residual  monitoring  plot  remaining  closer  to  the  “Truth  Data”  than 
the  basic  EKF  range  update  algorithm  shown  in  the  “Standard”  plot.  The  next 
paragraph  details  the  effect  of  the  Sage-Husa  algorithm. 

The  Sage-Husa  algorithm  provides  a  variety  of  results.  The  memory  factor  of 
0.005  is  applied  to  the  post-processing  of  each  of  the  four  data  collections.  In  Fig¬ 
ure  4.26  (a),  Sage-Husa  provides  a  slight  improvement  in  drift  constraint  during  the 
moving  portion  of  the  trajectory.  This  is  evident  overall  by  the  Sage-Husa  plot  fol¬ 
lowing  the  truth  data  more  closely  than  the  standard  or  residual  monitoring  plots. 
The  Sage-Husa  algorithm  appears  to  further  corrupt  the  trajectories  viewed  in  Fig¬ 
ures  4.26  (b)  and  4.26  (c).  This  is  due  to  slightly  incorrect  estimation  of  the  bias 
of  the  range  measurements  and  uncertainty  of  the  range  model.  The  signihcant  tra¬ 
jectory  corruption  viewed  in  Figure  4.26  (d)  is  due  to  the  large  negative  range  bias 
estimated  at  the  beginning  of  the  trajectory  from  the  large  negative  range  residuals. 
Figure  4.26  (d)  displays  a  weakness  of  the  Sage-Husa  algorithm,  where  the  negative 
bias  estimated  towards  the  beginning  of  the  trajectory,  as  depicted  in  Figure  4.25,  is 
not  corrected  to  prevent  corruption  of  the  remaining  trajectory. 

The  results  of  residual  monitoring  and  the  Sage-Husa  adaptive  algorithm  dis¬ 
play  pros  and  cons  for  each  method.  Residual  monitoring  proves  to  be  effective  at 
further  reducing  trajectory  drift  and  corruption  by  rejecting  range  residuals  which 
fall  outside  the  3-a  threshold.  However,  when  all  residuals  remain  under  the  thresh¬ 
old,  residual  monitoring  has  no  effect  beyond  the  basic  EKF  range  update  algorithm. 
The  Sage-Husa  algorithm  is  shown  to  provide  a  variety  of  results.  Depending  on  the 
selected  memory  factor  and  particular  data  set,  a  reduction  or  increase  in  trajectory 
drift  is  observed.  A  signihcant  contribution  from  the  Sage-Husa  algorithm  is  run¬ 
time  estimation  of  the  range  model  uncertainty  which  allows  the  hlter  to  account  for 
variations  in  the  uncertainty  of  the  incoming  range  measurements. 
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This  concludes  analysis  of  the  outdoor  data  collections.  Lack  of  proper  camera 
calibration  due  to  time  constraints  renders  the  outdoor  collected  imagery  unusable  in 
the  EKF.  Proper  camera  calibration  is  obtained  for  the  indoor  data  collections  which 
allows  the  indoor  imagery  to  be  included  in  EKF  post-processing.  The  observations 
and  analysis  presented  for  the  outdoor  data  sets  are  used  to  rehne  and  tune  the  navi¬ 
gation  system  for  the  indoor  data  collections.  The  next  section  presents  observations 
and  analysis  for  the  indoor  moving  data  collections. 

4.3  Indoor  Moving  Analysis 

This  section  presents  results  and  analysis  for  the  moving  data  collections  per¬ 
formed  indoors.  Setup  for  the  two  groups  of  indoor  collections  is  presented  in  Sec¬ 
tion  3.2.4.  Results  from  residual  monitoring  and  the  Sage-Husa  adaptive  algorithm 
are  presented  in  Section  4.3.1.  Analysis  of  the  trajectory  estimate  with  data  from  the 
stereo  imaging  system  are  presented  in  Section  4.3.2. 

Four  data  collections  are  performed  for  each  of  the  two  indoor  data  collect 
groups.  The  hrst  group  is  labeled  north-south  (NS)  and  the  second  group  is  labeled 
square  (SQR)  due  to  the  square-shape  of  the  trajectory.  Trajectories  for  the  NS  data 
sets  with  the  “a”  designation  begin  at  the  upper,  or  north,  surveyed  marker.  NS 
trajectories  with  the  “b”  designation  begin  at  the  lower,  or  south,  surveyed  marker. 
The  length  of  each  indoor  moving  data  collection  is  contained  in  Table  4.8. 

Table  4.8:  Total  Stationary  and  Moving  time  of  the  Indoor  Moving  Data  Sets 


Data  Collection 

Total  Time 

NSMa 

317  s 

NSMb 

305  s 

NS_2a 

301  s 

NS_2b 

318  s 

SQR_1 

440  s 

SQR_2 

444  s 

SQR_3 

439  s 

SQR_4 

440  s 
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The  analysis  of  the  outdoor  data  sets  in  Section  4.2  provides  a  post-processing 
baseline  for  the  indoor  data  sets.  This  base-line  consists  of  INS  data,  hlter  bias  esti¬ 
mation  enabled,  2-minute  hlter  alignment,  range  measurements,  and  vertical  channel 
constraint  data.  This  base  EKF  setup  is  referenced  in  the  following  plots  with  the 
label  “Standard.”  The  vertical  channel  constraint  data  is  generated  from  the  altitude 
of  the  indoor  surveyed  marker  at  each  trajectory  starting  position. 

The  alignment  data  consists  of  the  starting  trajectory  position  as  dehned  by  the 
indoor  surveyed  marker,  the  velocity  is  dehned  as  zero,  and  the  attitude  is  taken  from 
the  SPAN.  As  presented  in  Section  3.1,  the  SPAN  is  primarily  an  INS-aided  DGPS 
navigation  system,  but  when  GPS  is  not  available  the  SPAN  continues  to  estimate 
the  attitude  of  the  navigation  system  based  oh  of  the  HG1700  INS  measurements.  It 
is  important  to  note  that  the  HG1700  measurements  used  by  the  SPAN  to  estimate 
the  attitude  are  the  same  INS  measurements  used  by  the  EKF  navigation  system 
developed  in  this  thesis.  However,  the  SPAN  uses  additional  algorithms,  not  discussed 
in  this  thesis,  to  help  correct  and  remove  the  INS  drift.  This  does  not  constitute  a 
“truth”  data  source,  but  provides  a  rehned  estimate  of  the  navigation  system  attitude. 
Next,  range  residual  analysis  is  summarized  for  the  indoor  data  collections. 

The  average  standard  deviation  of  the  indoor  stationary  data  collections,  pre¬ 
sented  Section  4.1.2,  is  63  meters.  This  value  is  used  as  the  initial  radio  range  model 
uncertainty.  Residual  analysis  is  performed  for  all  eight  of  the  indoor  data  collections. 
The  radio  range  model  uncertainty  is  adjusted  to  120  meters  based  on  the  results  from 
all  eight  data  sets.  This  value  reflects  the  increase  in  range  measurement  error  due 
to  RE  interference.  The  indoor  results  continue  with  residual  monitoring  and  the 
Sage-Husa  algorithm  discussed  in  the  next  section. 

Residual  Monitoring  and  Sage-Husa.  Residual  monitoring  and  the 
Sage-Husa  adaptive  algorithm  have  a  variety  of  effects  on  the  outdoor  moving  data 
results  in  Section  4.2.4.  Reduction  in  the  trajectory  drift  from  either  algorithm  was 
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shown  to  be  dependent  on  the  uncertainty  of  the  range  measurements  and  the  drift 
already  present  in  the  INS  data. 

This  section  presents  analysis  and  results  from  residual  monitoring  and  the  Sage- 
Husa  algorithm  for  both  groups  of  indoor  data  collections.  Truth  trajectory  data  is 
not  available  for  the  indoor  moving  data  set  due  to  a  lack  of  GPS  satellite  visibility. 

As  mentioned  in  Section  4. 2. 4. 2,  the  Sage-Husa  algorithm  performance  is  sensi¬ 
tive  to  the  memory  factor  d  and  the  uncertainty  of  the  measurement  in  each  data  set. 
Due  to  the  increased  uncertainty  for  the  indoor  range  measurements,  the  Sage-Husa 
memory  factor  is  re-tuned  for  indoor  use. 

To  tune  the  Sage-Husa  memory  factor  for  all  of  the  indoor  moving  data  sets,  the 
resulting  post-processed  trajectories  for  a  range  of  memory  factors  are  overlaid  on  the 
indoor  hallway  map.  From  visual  inspection,  the  best  memory  factor  is  chosen  based 
on  the  reduction  of  trajectory  drift  and  alignment  of  the  post-processed  trajectory 
end  point  with  the  true  end  point.  The  memory  factors  ranged  as  high  as  0.4  and  as 
low  as  0.07  with  the  majority  of  values  clustering  around  0.1.  The  memory  factors 
above  0.1  did  not  produce  negative  range  model  uncertainty  estimates  due  to  the 
large  initial  range  model  uncertainty  of  120  meters.  The  value  of  0.09  is  chosen 
as  a  compromise  between  the  eight  indoor  moving  data  runs.  This  value  provides 
a  reduction  in  trajectory  error  for  each  indoor  moving  data  set.  As  mentioned  in 
Section  4. 2. 4. 2  for  the  outdoor  moving  data  set,  tuning  the  Sage-Husa  algorithm  over 
the  entire  indoor  data  set  provided  positive  results  for  these  data  sets.  The  more 
robust  tuning  strategy  would  tune  off  of  a  subset  of  the  outdoor  data  sets  and  then 
evaluate  this  memory  factor  on  the  remaining  data  sets.  The  Sage-Husa  algorithm 
is  potentially  over-tuned  for  the  indoor  data  sets  based  on  the  tuning  methodology 
applied  in  this  thesis. 

The  results  and  analysis  are  divided  into  two  sections.  Section  4. 3. 1.1  covers 
the  NS  data  sets  and  Section  4. 3. 1.2  covers  the  SQR  data  sets. 
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4-3. 1.1  NS  Data  Sets.  Figure  4.27  presents  horizontal  trajectory  com¬ 
parison  for  the  indoor  moving  NS  data  set.  The  results  from  the  base  post-processing 
setup,  residual  monitoring,  and  Sage-Husa  adaptive  algorithm  are  contained  in  Fig¬ 
ure  4.27. 

The  first  observation  is  the  large  drift  present  even  with  the  vertical  channel 
constraint  and  range  measurements.  A  signihcant  source  of  error  is  the  initial  attitude 
estimate  provided  by  the  SPAN.  If  the  initial  attitude  estimate  is  incorrect,  the  hlter 
will  estimate  incorrect  biases  which  adds  additional  error  to  the  entire  trajectory. 

Residual  monitoring  does  not  provide  further  drift  constraint  due  to  the  high 
range  model  uncertainty.  The  range  residuals  do  not  exceed  the  3-a  threshold  which 
allows  all  of  the  range  measurements  to  effect  the  trajectory. 

The  Sage-Husa  algorithm  provides  a  more  notable  reduction  in  trajectory  drift. 
During  the  moving  portions  of  the  trajectory,  the  range  measurement  uncertainty 
increases  due  to  changing  RF  interference  conditions.  As  the  navigation  system  travels 
along  the  reference  paths  shown  in  each  hgure,  the  various  building  walls  and  doors 
interfere  with  the  RF  signals.  During  the  stationary  portions  of  the  trajectory,  at  the 
very  beginning  and  end,  changes  in  RF  interference  are  signihcantly  reduced.  The 
Sage-Husa  algorithm  is  able  to  reduce  the  range  model  uncertainty  as  it  estimates  the 
range  bias  error,  providing  stronger  position  updates  to  the  EKF.  This  results  in  the 
constraint  of  the  horizontal  trajectory  drift  shown  most  notably  in  Figures  4.27  (a) 
and  4.27  (c). 

It  is  also  observed  that  less  RF  interference  is  present  at  the  south  end  of  the 
hallway  (bottom  of  the  graphs)  as  compared  with  the  north  end  of  the  hallway  (top 
of  the  graphs).  The  Sage-Husa  algorithm  reduces  the  EKF  range  model  uncertainty 
where  there  is  lower  RF  interference.  The  higher  RF  interference  experienced  at  the 
north  end  of  the  hallway  results  in  the  larger  Sage-Husa  uncertainty  and  horizontal 
trajectory  drift  observed  in  Figures  4.27  (b)  and  4.27  (c). 
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Figure  4.27:  Residual  Monitoring  and  Sage-Husa  Horizontal  Trajectory  Comparison 
for  North-South  Indoor  Data  Sets.  Residual  monitoring  does  not  provide  additional 
drift  constraint  due  to  the  high  range  model  uncertainty.  Sage-Husa  provides  a  reduc¬ 
tion  in  drift  by  reducing  the  range  model  uncertainty  for  range  measurements  with 
reduced  RF  interference  as  shown  in  Figures  4.27  (a)  and  4.27  (c). 
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Figure  4.28:  Filter  Position  States  Uncertainty  Comparison  for  Indoor  Moving  Data 
Set  “North-South  la.”  Notice  that  the  hlter  uncertainty  for  the  north  and  east  axes 
is  similar  for  each  set  of  algorithms. 

Analysis  of  the  hlter  uncertainty  for  each  of  the  NS  data  sets  reveals  the  radio 
geometry  does  not  strongly  effect  the  Kalman  hlter  uncertainty  in  the  position  states 
for  the  NS  data  sets.  The  large  range  uncertainty  and  short  length  of  the  trajectory 
minimizes  the  ehect  of  poor  geometry.  Figure  4.28  presents  the  hlter  uncertainty  for 
indoor  moving  data  set  “North-South  la.”  The  hlter  uncertainty  for  the  horizontal 
north  and  east  axes  show  similar  uncertainties  for  each  set  of  algorithms. 

4-3. 1.2  SQR  Data  Sets.  Figure  4.29  contains  results  for  the  SQR 
group  of  indoor  moving  data  collections.  The  ehects  of  residual  monitoring  and  the 
Sage-Husa  adaptive  algorithm  on  the  SQR  moving  data  sets  are  discussed  in  this 
section. 

Large  trajectory  drift  is  noted  in  all  four  of  the  SQR  data  sets.  The  combination 
of  attitude  errors  present  for  the  initial  hlter  alignment,  the  increased  length  of  the 
SQR  data  collections  as  compared  with  the  NS  data  sets,  and  increased  RF  inter- 
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Figure  4.29:  Residual  Monitoring  and  Sage-Husa  Horizontal  Trajectory  Compar¬ 
ison  for  Square  Data  Sets.  Residual  monitoring  provides  some  drift  constraint  in 
Figure  4.29  (d),  but  does  not  affect  the  remaining  data  sets  due  to  the  high  range 
model  uncertainty.  Sage-Husa  provides  signihcant  drift  constraint,  allowing  range 
measurements  with  low  RF  interference  to  more  strongly  correct  the  hlter  position 
estimate. 
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ference  all  contribute  to  the  trajectory  drift.  The  increase  in  RF  interference  conies 
from  the  additional  layers  of  building  wall  and  doors  which  come  between  the  DH500 
radios  as  the  navigation  system  moves  along  the  reference  path. 

Residual  monitoring  has  a  notable  effect  on  Figure  4.29  (d).  Several  of  the  range 
residual  exceed  the  3-a  threshold  and  are  rejected.  This  results  in  a  temporary  reduc¬ 
tion  in  trajectory  drift.  However,  the  residual  monitoring  trajectory  in  Figure  4.29  (d) 
still  drifts  signihcantly.  Residual  monitoring  does  not  affect  the  trajectory  drift  in  Fig¬ 
ures  4.29  (a),  4.29  (b),  and  4.29  (c)  due  to  the  range  residual  not  exceeding  the  3-a 
threshold. 

The  Sage-Husa  adaptive  algorithm  signihcantly  reduces  the  trajectory  drift  for 
all  four  of  the  SQR  data  sets.  As  the  RF  interference  increases  during  the  middle 
portion  of  the  trajectory,  Sage-Husa  increases  range  model  uncertainty,  causing  the 
EKF  to  signihcantly  reduce  the  weight  placed  on  these  high  RF  interference  range 
measurements.  This  reduces  the  position  error  contributed  by  the  range  measure¬ 
ments.  Towards  the  end  of  the  trajectory,  as  RF  interference  decreases,  the  Sage- 
Husa  estimate  of  the  range  model  uncertainty  also  decreases.  This  allows  the  range 
measurements  with  lower  RF  interference  to  more  strongly  update  the  hlter  position 
estimate,  helping  to  reduce  the  trajectory  drift. 

Figure  4.30  presents  the  Sage-Husa  analysis  for  indoor  moving  data  collection 
“Square  3”  as  shown  in  Figure  4.29  (c).  Note  how  the  hlter  uncertainty,  or  mea¬ 
surement  noise  variance,  increases  as  the  RF  interference  increases.  Also  note  how 
the  range  bias,  or  measurement  noise  mean,  tracks  with  the  positive  mean  of  the 
range  bias.  This  estimated  bias  is  subtracted  from  the  range  residual  before  updating 
the  EKF  position  states.  The  Sage-Husa  adaptive  algorithm  signihcantly  reduces  the 
trajectory  drift  providing  a  more  accurate  position  estimate. 

The  longer  and  more  complex  “Square”  trajectory  introduces  more  variation  to 
the  Kalman  hlter  position  uncertainty.  Figure  4.31  presents  the  Kalman  hlter  position 
uncertainty  for  the  “Square  1”  data  set.  For  the  standard  and  residual  monitoring 
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Relative  GPS  Time  [sec] 

Figure  4.30:  Sage-Husa  Estimate  Analysis  for  Data  Collection  “Square  3.”  The 

range  model  uncertainty  estimate,  measurement  noise  variance,  reduces  the  effect  of 
very  poor  range  measurements  through  the  increase  of  the  model  uncertainty.  Less 
RF  interference  is  present  towards  the  end  of  the  trajectory  and  the  significant  reduc¬ 
tion  in  model  uncertainty  allows  these  range  measurements  to  strongly  constrain  the 
trajectory  drift. 
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Figure  4.31:  Filter  Position  States  Uncertainty  Comparison  for  Indoor  Moving  Data 
Set  “Square  1.”  Notice  that  the  hlter  uncertainty  for  the  north  and  east  axes  varies 
based  on  the  algorithms  employed.  The  north  axis  uncertainty  is  larger  than  the  east 
axis  for  residual  monitoring  and  the  standard  algorithm. 

algorithms,  the  north  axis  uncertainty  is  generally  larger  that  the  respective  east  axis. 
The  geometry  of  the  radio  positions  is  a  primary  source  of  this  difference.  Spacing 
the  outdoor  radios  further  apart,  placing  the  northern  base  station  further  north 
and  the  southern  base  station  further  south,  would  improve  the  geometry  when  the 
indoor  sensor  suite  moves  furthest  away  from  the  outdoor  radios.  Notice  also  that 
addition  of  the  Sage-Husa  algorithm  signihcantly  changes  the  Kalman  hlter  position 
uncertainties  for  the  north  and  east  axes.  With  the  Sage-Husa  algorithm,  the  range 
residual  covariance  value  is  more  dependent  on  the  current  uncertainty  of  the  incoming 
range  measurements  than  the  radio  system  geometry. 

The  results  presented  for  both  the  NS  group  and  SQR  group  contain  signihcant 
trajectory  error  when  compared  to  their  respective  reference  paths.  The  INS  drift 
is  constrained  with  the  range  measurements  and  Sage-Husa  adaptive  algorithm,  but 
precision  navigation  within  the  building  is  not  possible  with  the  remaining  trajectory 
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drift.  Results  and  analysis  using  imagery  from  the  stereo  camera  system  are  presented 
in  the  next  section. 

4.3.2  Stereo  Image  Aiding.  Image-aiding  provides  position,  velocity  and  at¬ 
titude  information  based  on  image  features  tracked  within  view  of  the  stereo  camera 
system.  This  section  presents  post-processed  results  with  the  addition  of  image-aiding 
to  the  EKF  setup.  Each  of  the  eight  indoor  moving  data  sets  is  post-processed  with 
INS  data,  bias  estimation,  2- minute  stationary  alignment  data,  vertical  channel  con¬ 
straint  data,  range  measurements,  and  stereo-camera  imagery.  The  Sage-Husa  algo¬ 
rithm  is  also  included  in  the  EKF  radio  range  model  due  to  the  additional  trajectory 
drift  reduction  afforded  by  its  estimation  of  the  range  bias  and  model  uncertainty. 

The  EKF  post-processing  setup  contains  three  parameters  that  tune  the  image- 
aiding  algorithm.  The  minimum  feature  scale  is  set  to  zero  to  accept  all  available 
image  features  found  by  the  camera  system.  To  increase  the  efficiency  of  the  EKF 
algorithm,  the  maximum  number  of  tracked  image  features  is  limited  to  10.  Image 
features  are  dropped  when  the  tracked  feature  moves  out  of  the  camera’s  view.  The 
number  of  additional  features  added  to  fill  empty  tracking  slots  is  set  to  five  to  reduce 
the  image  feature  turn-over  rate  of  the  image-aiding  algorithm.  Reducing  this  turn¬ 
over  rate  provides  more  stable  feature  tracking  which  helps  improve  the  position, 
velocity,  and  attitude  updates  generated  by  the  image-aiding  system. 

Section  4.3.2. 1  contains  observations  and  analysis  for  the  NS  data  sets.  Results 
and  observations  for  the  SQR  data  set  are  presented  in  Section  4. 3. 2. 2. 

4.3.2. 1  NS  Data  Sets.  Figure  4.32  presents  results  for  the  NS  group 
of  the  indoor  moving  data  collections.  Analysis  and  observations  for  the  addition  of 
image-aiding  from  the  stereo  camera  system  are  presented  next. 

The  addition  of  image-aiding  to  EKF  post-processing  produces  a  variety  of 
results.  Figures  4.32  (a)  and  4.32  (d)  show  the  addition  of  fnrther  drift  to  the  post- 
processed  trajectory.  The  poor  availablity  of  features  present  in  the  building  hallway 
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(b)  North-South  lb 


Figure  4.32:  Image- Aiding  Horizontal  Trajectory  Comparison  for  North-South  data 
sets.  The  addition  of  image-aiding  provides  additional  variety  in  both  the  reduction  of 
and  addition  to  existing  trajectory  drift.  The  sparse,  flat  hallways  provide  a  minimal 
number  of  image  features  for  the  image-aiding  system  to  track  resulting  in  overall 
poor  performance. 
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contribute  significantly  to  the  increase  in  drift.  Sometimes  the  image-aided  system 
is  able  to  lock-on  the  the  available  features  and  provide  filter  updates  as  is  evident 
in  the  reduction  of  drift  shown  in  Figures  4.32  (b)  and  4.32  (c)  and  other  times  the 
system  incorrectly  tracks  image  features  adding  significant  additional  position  error 
as  shown  in  Figures  4.32  (a)  and  4.32  (d). 

Another  source  of  image-aiding  error  is  objects  temporarily  obstructing  the 
camera’s  view  of  the  hallway.  Obstruction  occurs  when  a  person  walks  into  the 
camera’s  view.  This  occurred  to  various  degrees  in  each  of  the  indoor  data  sets.  Other 
obstructions  may  consist  of  the  blockage  of  one  or  both  cameras  due  to  an  obstacle  in 
the  hallway.  Whenever  a  feature  moves  out  of  the  camera’s  view  or  is  blocked  from 
the  camera’s  view,  the  image-aiding  system  stops  tracking  the  feature.  Fewer  tracked 
features  results  in  reduced  accuracy  in  the  position,  velocity,  and  attitude  updates 
provided  to  the  EKF  by  the  image-aiding  system. 

4.3. 2. 2  SQR  Data  Sets.  Figure  4.33  displays  the  results  for  the  SQR 
group  of  indoor  moving  data  collections.  The  outcome  of  adding  image-aiding  to  EKF 
post-processing  is  discussed  in  this  section. 

The  SQR  trajectory  introduces  90°  turns  around  the  hallway  corners  into  the 
reference  path.  After  each  corner  is  rounded,  the  navigation  system  is  stationary 
for  10-15  seconds  to  let  the  image-aiding  system  observe  the  new  hallway  to  find  new 
features  to  track.  As  is  presented  in  Figure  4.33  a  wide  variety  of  results  are  obtained. 

The  image-aiding  system  retains  features  that  move  out  of  the  camera’s  view 
or  are  block  from  view  for  several  seconds.  This  is  done  to  facilitate  smooth  fea¬ 
tures  tracking.  When  a  hallway  corner  is  rounded,  the  image-aiding  system  ends  up 
dropping  all  existing  tracked  features.  This  process  of  dropping  and  reacquiring  new 
features  is  a  significant  source  of  image- aiding  error.  Figures  4.33  (a),  4.33  (b),  and 
4.33  (c)  present  image-aided  trajectories  that  display  the  effect  of  rounding  a  hallway 
corner,  where  the  image-aiding  system  is  unable  to  acquire  good  new  features  to  track. 
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Figure  4.33:  Image-Aiding  Horizontal  Trajectory  Comparison  for  the  SQR  Data 

Set.  The  results  from  image-aiding  are  varied.  Rounding  hallway  corners  during  the 
trajectory  present  a  signihcant  source  of  image-aiding  error.  Camera  view  obstructions 
such  as  people  and  objects  in  the  hallways  cause  further  image-aiding  errors  which 
results  in  a  reduction  of  trajectory  drift. 
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The  best  image-aiding  results  obtained  for  this  thesis  are  shown  in  Figure  4.33  (d). 
Several  factors  contributed  to  the  signihcant  trajectory  drift  constraint  displayed  in 
this  plot.  The  image- aiding  system  was  able  to  acquire  and  track  sufficient  image  fea¬ 
tures  to  provide  accurate  position,  velocity,  and  attitude  updates  to  the  EKF.  Also, 
a  lack  of  camera  obstructions  was  present  in  this  data  set.  Very  few  people  blocked 
the  camera’s  view  during  the  entirety  of  the  “Square  4”  data  collection. 

Analysis  of  image-aiding  performance  for  both  the  “North-South”  and  “Square” 
data  sets  without  range  measurements  showed  almost  identical  performance  to  the 
range  and  image-aided  trajectories  shown  in  Figures  4.32  and  4.33.  This  indicates  the 
range  measurements  do  not  signihcantly  effect  the  post-processed  trajectory  when  the 
image-aiding  system  is  enabled. 

The  results  presented  in  this  chapter  for  both  the  indoor  and  outdoor  data 
collections  display  a  wide  variety  of  results.  Constraint  of  trajectory  drift  is  shown 
most  clearly  by  the  Sage-Husa  adaptive  method  and  selected  image-aided  data  sets. 
The  hnal  evaluation  of  the  algorithms  and  aiding  techniques  utilized  in  this  navigation 
system  are  presented  in  the  next  chapter. 
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V.  Conclusions  and  Future  Work 


This  chapter  presents  the  conclusions  of  this  thesis  and  recommendations  for 
future  work.  Section  5.1  presents  an  evaluation  of  the  various  algorithms  and 
aiding  techniques  used  in  the  navigation  system.  Improvements  for  the  navigation 
system  and  recommendations  for  next  steps  in  research  are  presented  in  Section  5.2. 
Section  5.3  contains  the  summary  for  this  thesis. 

5.1  Conclusions 

Analysis  of  the  navigation  system  began  with  characterization  of  the  outdoor 
stationary  radio  performance  presented  in  Section  4.1.1.  As  expected,  the  range  error 
histograms  displayed  a  very  low  number  of  outliers,  which  was  due  to  very  low  RF 
interference.  The  predominant  outdoor  range  error  source  was  noise  present  in  the 
radio  ranging  system.  This  noise  was  shown  to  be  Gaussian.  The  average  RMS  range 
error  of  8  meters  was  used  for  the  first  EKF  radio  range  model  uncertainty. 

Next,  the  navigation  system  was  tested  by  performing  several  outdoor,  moving 
data  collections.  During  filter  tuning,  the  EKF  range  model  uncertainty  was  adjusted 
to  55  meters.  The  results  show  the  range  measurements  provide  significant  trajectory 
drift  constraint.  Position  errors  upwards  of  1000  meters  with  INS-only  navigation 
were  reduced  to  25-50  meters  when  the  range  measurements  were  applied  along  with 
filter  bias  estimation.  In-depth  results  are  presented  in  Section  4.2. 

Residual  monitoring  was  added  to  the  EKF  range  update  algorithm  to  help 
improve  the  drift  constraint  provided  by  the  range  measurements.  The  results  in 
Section  4.2.4  showed  this  algorithm  improved  the  average  RMS  trajectory  error  by  5 
meters  for  data  sets  with  large  range  outliers. 

The  Sage-Husa  adaptive  algorithm  was  tested  next,  with  results  also  contained 
in  Section  4.2.4.  After  tuning,  the  Sage-Husa  algorithm  was  shown  to  provide  an 
additional  0.5  to  2  meters  reduction  in  the  average  trajectory  error  for  each  data 
collection. 
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The  navigation  system  was  then  moved  indoors  to  test  the  range  drift  con¬ 
straint  capability  in  an  environment  with  high  RF  interference  .  A  stationary  range 
performance  characterization  was  performed  for  the  indoor  setup.  The  range  error 
histograms,  presented  in  Section  4.1.2,  show  signihcant  range  error  from  the  RF  in¬ 
terference.  The  average  RMS  error  of  63  meters  is  used  to  set  EKF  range  model 
uncertainty  for  the  indoor,  moving  data  collections. 

The  navigation  system  was  then  tested  by  performing  several  indoor,  moving 
data  collections.  The  results  are  presented  in  Section  4.3.  During  the  moving  data 
collects,  RF  interference  changes.  This  results  in  a  large  variation  in  the  range  error. 
The  range  model  uncertainty  was  adjusted  to  120  meters  to  account  for  this  variation. 
The  radio  range  measurements  provided  trajectory  drift  constraint,  but  at  a  reduced 
level.  This  was  expected  because  of  the  increased  RF  interference. 

Next,  the  Sage-Husa  adaptive  algorithm  was  tested  on  the  indoor,  moving  data 
sets,  with  results  presented  in  Section  4.3.1.  After  tuning  Sage-Husa  for  the  indoor 
data  sets,  decreases  in  trajectory  drift  upwards  of  100  meters  were  observed. 

Finally,  the  image-aiding  system  was  incorporated  into  the  previous  set  of  sen¬ 
sors.  When  the  image-aiding  system  found  good  features,  the  trajectory  constraint 
between  3  to  12  meters  was  observed.  In-depth  results  are  presented  in  Section  4.3.2. 

Overall,  the  radio  range  measurements  constrained  the  inertial  sensor  drift. 
The  addition  of  Sage-Husa  provided  further  reduction  in  trajectory  error.  The  image- 
aiding  system  showed  the  strongest  drift  constraint  performance  when  good  image 
features  were  found.  The  next  section  discusses  areas  of  future  work  to  further  improve 
the  navigation  system  presented  in  this  thesis. 

5.2  Future  Work 

This  section  discusses  four  areas  of  future  work  which  would  provide  further 
advancement  of  the  navigation  capability  presented  in  this  thesis.  The  hrst  area  is 
the  estimation  of  errors  due  to  RF  interference.  A  strong  potential  source  of  RF 
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interference  for  the  DH500  radio  system  is  RF  multipath.  There  exist  in  [2,  3,  7, 12] 
several  scatter  models  and  algorithms  to  account  for  multipath  effects.  The  results 
contained  in  [2,3]  show  the  scatterer  models  to  be  robust  in  several  different  scatterer 
environment  models  providing  notable  reduction  in  multipath  position  error.  These 
multipath  estimators  were  not  implemented  in  this  thesis  due  to  time  constraints. 

The  second  area  is  implementation  of  a  real  baro-altimeter.  The  simulated 
baro-altimeter  used  in  this  thesis  to  constrain  the  vertical  navigation  drift  required 
GPS  or  surveyed  markers.  The  choice  to  simulate  the  baro-altimeter  data  was  made 
to  simplify  the  navigation  system  to  focus  on  implementation  of  the  radio  ranging 
system.  A  real  baro-altimeter  would  permit  the  navigation  system  to  estimate  the 
altitude  regardless  of  the  environment.  Tracking  altitude  changes  between  building 
floors  is  just  one  example  of  the  expanded  flexibility  a  real  baro-altimeter  would 
provide. 

The  third  area  of  further  research  is  implementation  of  an  unscented  Kalman 
hlter  instead  of  the  EKF.  Implementation  of  the  unscented  Kalman  hlter  would  not 
require  linearization  of  the  vehicle  and  sensor  model  as  is  required  for  the  EKF.  The 
linearization  process  removes  non-linearities  in  the  vehicle  and  sensor  which  introduces 
model  error  into  the  hlter.  Direct  implementation  of  the  non-linear  models  allows  the 
unscented  Kalman  hlter  to  provide  a  more  accurate  estimate  of  the  system  states. 

Fourth,  the  Sage-Husa  adaptive  algorithm  showed  signihcant  promise  in  tra¬ 
jectory  drift  reduction.  However,  the  dual  memory  factor  tuning  performed  in  this 
thesis  to  accommodate  the  low  and  high  RF  interference  environments  requires  the 
user  to  switch  between  factors  depending  on  the  environment.  This  can  be  improved 
by  determining  a  single  Sage-Husa  memory  factor  that  provides  drift  reduction  for 
both  the  outdoor  and  indoor  environments. 


119 


5.3  Closing 

The  navigation  system  developed  in  this  thesis  has  been  shown  to  provide  no¬ 
table  INS  drift  constraint  through  the  use  of  the  DH500  radio  system.  The  addition 
of  the  Sage-Husa  adaptive  algorithm  and  image-aiding  system  provided  additional 
sources  of  drift  constraint.  Shown  to  work  in  environments  with  both  high  and  low 
RF  interference,  this  research  presents  a  viable  navigation  system  that  enables  preci¬ 
sion  navigation  for  the  warfighter  in  GPS-denied  environments. 
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